# 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?

• I know this is kinda old post, but did you manage to get any good results from the accelerometer ? – M.Saeed Dec 21 '19 at 23:42
• @M.Saeed I used the imufilter from matlab, kalman fil implementation, to get good results using the accelerometer and gyroscope. mathworks.com/help/fusion/ref/imufilter-system-object.html – angryip Dec 24 '19 at 13:47