I am applying Kalman Filter to estimate the states of a particle moving along x direction. The states of the particle at any discrete time $t = k$ are $\left[x_k \hspace{3pt} \dot{x}_k \hspace{3pt} \ddot{x}_k\right]^T$. However I can only observe $x_k$ and $\dot{x}_k$ and there is no observation for $\ddot{x}_k$. We can also see that $x$ and $\dot{x}_k$ have no projection along $\ddot{x}_k$. So, for any given measurement, there is no direct information about $\ddot{x}_k$. My aim is to find $\ddot{x}_k$ as accurately as possible. There are two possible ways:
- Kalman Filter( KF): Since the observed measurements do not have any projection along the $\ddot{x}_k$, the estimate of $\ddot{x}_k$ will be very noisy and may be way off from the true value of $\ddot{x}_k$.
- Use Kalman Filter as in step 1 to do the estimation for all parameters, but in addition to that, use first order low pass filter(LPF) to get a more accurate and less noisy estimate of $\ddot{x}_k$ i.e. $\ddot{x}_k = \alpha \ddot{x}_{k-1} + \left(1-\alpha\right)\frac{\dot{x}_{k} - \dot{x}_{k-1}}{\Delta t}$
However, the value of $\ddot{x}_k$, so obtained, will not be used in the propagation or update step of KF and hence cannot influence the $x_k$ and $\dot{x}_k$ estimate. So, there will be inconsistency among the values $x_k$, $\dot{x}_k$ and the LPF based $\ddot{x}_k$. This may create problems.
Now, there is another way to get a more accurate estimate of $x$, $\dot{x}$ and $\ddot{x}$. But I am not sure about its validity. It goes like this:
- Basically, in addition to the observed states $x_k$ and $\dot{x}_k$ for Kalman Filter method, we can introduce a pseudo-observation of $\ddot{x}_k = \frac{\dot{x}_{k} - \dot{x}_{k-1}}{\Delta t}$, where $\Delta t$ is the time difference between $t = k$ and $t = k-1$. While practically, this value is $\ddot{x}_{k-1}$, this will maintain consistency among all the states.
I also understand that by introducing this pseudo-observation, I am forcing a mathematical relationship among acceleration, velocity and time, which may not exist for the given system and this is against the basic concept of filtering. So, does anyone think that this method of introducing the pseudo-observation is legit?
Kalman filter equations:
Propagation: \begin{equation*} \begin{bmatrix} \hat{x}_{k+1} \\ \hat{\dot{x}}_{k+1} \\ \hat{\ddot{x}}_{k+1} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2} \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x_{k} \\ \dot{x}_{k} \\ \ddot{x}_{k} \end{bmatrix} + \begin{bmatrix} \eta \frac{\Delta t^3}{6} \\ \eta \frac{\Delta t^2}{2} \\ \eta \Delta t \end{bmatrix}, \hspace{10pt} \eta \sim \mathcal{N}\left(0, Q\right) \end{equation*}
Update: \begin{equation*} \begin{bmatrix} z_1\\ z_2 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix}\begin{bmatrix} \hat{x}_{k+1} \\ \hat{\dot{x}}_{k+1} \\ \hat{\ddot{x}}_{k+1} \end{bmatrix} + \begin{bmatrix} \nu_1 \\ \nu_2 \end{bmatrix}\hspace{10pt}, \nu_i \sim\mathcal{N}\left(0, R_i\right) \end{equation*}