1
$\begingroup$

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:

enter image description here

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?

$\endgroup$

closed as off-topic by rob Jun 20 '18 at 15:49

  • This question does not appear to be about physics within the scope defined in the help center.
If this question can be reworded to fit the rules in the help center, please edit the question.

  • 2
    $\begingroup$ You'll almost certainly get better reception if you ask this question at the signal processing stackexchange site. That said, Kalman filters are not a magical bullet that remove the nasty doubly integrated white noise problem. Your statement "This also makes me believe that I need "more data" to use the kalman filter as opposed to only the acceleration" is spot on. You'll need a noisy measurement that somehow correlates with state. Without such measurements, a Kalman filter becomes the worst kind of integrator possible: essentially Euler's method, with noise. $\endgroup$ – David Hammen Jun 20 '18 at 10:48
  • $\begingroup$ @DavidHammen thanks for your feedback. I'll see if I can get in touch with the admins to transfer the question over. && Ya, that makes sense. I'm sure it won't get rid of the nasty double integration, but given that we can play with almost every stage of the calculation process, certainly more room to improve. $\endgroup$ – angryip Jun 20 '18 at 11:20