algorithms - Using the Kalman filter given acceleration to estimate position and velocity

Note : port from this post, until fully merged.

I am reading data from an accelerometer. I want to use this data to estimate velocity and position. Originally, I performed a double integration of acceleration to read this data, and as confirmed by thousands of other posts, I received exponential drift of error. As a result, web hunting has lead me to the Kalman filter. The general consensus is "Please don't use double integration. Use a filter, like the Kalman filter, Extended K filter, U K Filter, etc.. to get a better estimate"

And as a result, I understand why double integration doesnt perform as well as I imagined and why filtering is necessary. Searching on stack yields several similar questions, but my lack of understanding is potentially preventing me from solving the problem by myself.

On that note, here is the flow that I am attempting to use to estimate tracking position and velocity:

What data do I have?

I currently read the accelerometer x,y,z and gyro x,y,z data. To make a small proof of concept, I've placed the accelerometer on a flat surface. This flat surface moves up for a duration of 3 sec, and stops, at which point my data collection completes. So essentially, in the flow below, I am only using the accelerometer X axis as a data input ( for now ), as that is the only axis that is undergoing change in motion.

Where do I want to get?

Placing the accelerometer on a flat surface and moving it up, is a proof of concept case. I eventually want to get to a stage where I can twist and turn the accelerometer and still get the position. ( We can all dream ) . And if it doesn't work as well as I hope, oh well, I at least want to try so that I can observe what is happening. But before I get there, understanding how to use the Kalman filter on just one axis would be vital.

Previous state:

$X_{k-1}$ - Positon

$V_{k-1}$ - Velocity

This part makes sense to me. Considering that I know when I started moving the accelerometer device and when I stopped, I'd set these two values to $0$.

New predicted state:

$X_{kp} = AX_{k - 1} + Bu_{k} + W_k$

$X_{kp} = \begin{bmatrix} 1 & \Delta T \\ 0 & 1 \end{bmatrix} \begin{bmatrix} X_{k-1} \\ V_{k-1} \end{bmatrix} + \begin{bmatrix} \frac{1}{2}\Delta T^2 \\ \Delta T \end{bmatrix} \begin{bmatrix} a_{x_{k}} \end{bmatrix} + W_k $

If I work out the above equations, I can tell that they conform to the equations of motion,

$X_{kp} = \begin{bmatrix} X_{k-1} + V_{k-1}\Delta T \\ V_{k-1} \end{bmatrix} + \begin{bmatrix} \frac{1}{2}\Delta T^2a_{x_{k}} \\ \Delta Ta_{x_{k}} \end{bmatrix} + W_k $

$X_{kp} = \begin{bmatrix} X_{k-1} + V_{k-1}\Delta T + \frac{1}{2}\Delta T^2a_{x_{k}} \\ V_{k-1} + \Delta Ta_{x_{k}} \end{bmatrix} + W_k $

With that in mind, my predicted state $X_{kp}$ is always going to take past input for position and velocity. My predicted state is always going to take the current acceleration, not the previous one, to estimate the predicted state. So if I start reading my accelerometer data from an array, I'd use index of zero to get the acceleration, and $X_{k-1}$ and $V_{k-1}$ would be initially $0$ and later something else based on the rate of acceleration.

Process covariance matrix:

$P_{kp} = AP_{k-1}A^T + Q_k$

$P_{k-1} = \begin{bmatrix} \delta_{x}^2 & \delta_{x} \delta_{y} \\ \delta_{x} \delta_{y} & \delta_{y}^2 \end{bmatrix} $

$P_{k-1} = \begin{bmatrix} \Delta EX_{k-1}^2 & \Delta EX_{k-1} \Delta EV_{k-1} \\ \Delta EX_{k-1} \Delta EV_{k-1} & \Delta EV_{k-1}^2 \end{bmatrix} $

$\Delta EX_{k-1}$ - Process error in position

$\Delta EV_{k-1}$ - Process error in velocity

$P_{kp} = \begin{bmatrix} 1 & \Delta T \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \Delta EX_{k-1}^2 & \Delta EX_{k-1} \Delta EV_{k-1} \\ \Delta EX_{k-1} \Delta EV_{k-1} & \Delta EV_{k-1}^2 \end{bmatrix} \begin{bmatrix} 1 & 0\\ \Delta T & 1 \end{bmatrix} + Q_k$

If I work out the above equation, I get the following:

$P_{kp} = \begin{bmatrix} \Delta EX_{k-1}^2 + \Delta EX_{k-1} \Delta EV_{k-1} \Delta T & \Delta EX_{k-1} \Delta EV_{k-1} + \Delta EV_{k-1}^2 \Delta T \\ \Delta EX_{k-1} \Delta EV_{k-1} & \Delta EV_{k-1} ^2 \end{bmatrix} \begin{bmatrix} 1 & 0\\ \Delta T & 1 \end{bmatrix} + Q_k $

$P_{kp} = \begin{bmatrix} \Delta EX_{k-1}^2 + \Delta EX_{k-1} \Delta EV_{k-1} \Delta T + (\Delta EX_{k-1} \Delta EV_{k-1} + \Delta EV_{k-1}^2 \Delta T) \Delta T & \Delta EX_{k-1} \Delta EV_{k-1} + \Delta EV_{k-1}^2 \Delta T \\ \Delta EX_{k-1} \Delta EV_{k-1} + \Delta EV_{k-1}^2 \Delta T & \Delta EV_{k-1}^2 \end{bmatrix} + Q_k$

This covariance matrix would make sense to me, as the position and velocity have a direct relationship in error accumulation.

The Kalman Gain

$K = \frac{P_{kp}H^T}{HP_{kp}H^T + R}$

It would make sense to me to have the $H$ matrix as follows:

$H = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}$

Given that $H$ is an identity matrix, then $H^T$ should be the same as $H$.

It would make sense to me to construct my $R$ just like I constructed the $P_{k-1}$ matrix, except instead of using process covariance values, I'd use a different observational error. Here is what I think:

$OX_k$ - Observed error in position

$OV_k$ - Observed error in velocity

$R = \begin{bmatrix} OX_k^2 & OX_k OV_k \\ OX_k OV_k & OV_k^2 \end{bmatrix}$

These values would be the same over time, as $R$ holds no recursive state.

The new observation

$Y_k = CY_{k_{m}} + Z_k$

What I question when I get to this stage is how do I configure the $Y_{k_{m}}$ matrix. Considering that this is supposed to be the "new observed state", this is the only way I can think of wrt to getting the current state:

$Y_{k_{m}} = \begin{bmatrix} \frac{1}{2}\Delta T^2 + X_{k-1}\\ \Delta T + V_{k-1} \end{bmatrix} $

However, this equation would then be exactly the same as $X_{kp}$ given that $W_k = 0$. This then makes me believe that I am doing something wrong. This also makes me believe that I need "more data" to use the kalman filter as opposed to only the acceleration.

Questions

1. 
   

   Is how I am attempting to use the Kalman filter correct?

   
   

   i.e I have acceleration on some axis, and I want to get the positon and velocity from it. ( Eventually, I will have acceleration on two axis )

   
   


2. 
   

   If I am fairly close to being correct, how would I go about completing the $Y_{k_{m}}$ matrix?

   
   

Answer

There is “correct” and there is “correct”.

Your train of reasoning is in the right direction.

A KF in this case is still double integration. The filter has some advantages in that if you have your parameters tuned, you will know your error. The KF can also be run backwards in time as a KF smoother so knowledge of the ending position can be combined with the initial condition to improve your accuracy, but this is done off line.

Real accelerometers are a bit more complicated than the kind covered in text books. Accelerometers have bias and you have to account for correlated measurements. These aren’t problems because there are corrections that most KF books will cover, but they need to be addressed. I suggest you contact the manufacturer of your accelerometer and inquire if they have a KF related app note for their product. If they do, BINGO!

So, yes you are making reasonable choices but there are details and more details. As a causal online state estimator, it is fundamentally double integration as your problem is stated.