# Estimating velocity from known position and acceleration

I am stuck at modeling a system model, i.e. getting my state vector and input vector. My guess is that position and velocity are state vector and acceleration is input vector. My 2nd guess is that all three quantities are in state vector and none in input vector.

So... what is state vector and what is input vector in my case?

--

I get measurements from position sensor and acceleration sensor. Everything is happening in 1D, for example on a straight line. I want to merge these readings (and remove the noise) to get an estimation of velocity for each timestep.

These equations describe the system; I am not sure if they're modeled right though. If I understand correctly it's safe to predict that acceleration is constant (even though in reality it changes) - because process covariance matrix fixes this assumption (right?).

I have also some sample data to work with (input values aren't noised here for simplicity):

 time    pos     acc      what I should get as output (velocity)
[0.0s]  0.000, -0.000  | 18.850
[0.1s]  1.885, -0.113  | 18.850
[0.2s]  3.768, -0.227  | 18.839
[0.3s]  5.650, -0.340  | 18.816
[0.4s]  7.528, -0.452  | 18.782
[0.5s]  9.401, -0.565  | 18.737


For better communication I'm creating a new answer but should be treated as a comment to the first answer. Jason you've already helped me tremendously and I really am grateful for your time. I still have problems with this though - the results from Kalman Filter are not as expected. May you find the time please review the following, thanks. I already owe you a beer or two (or coffies if you like) - if you have paypal contact me on primoz[a t]codehunter.eu :)

I've implemented the model Jason had proposed in first answer. I added the jerk as 4th state variable. After hours of reviewing I decided to come back here for help. The values I get out of KF aren't as expected. Table below represents the data from first 10 iterations of algorithm. Notice how jerk is increasing each time step thus making other estimates wrong. After one second the difference between real acceleration and estimated is more than 1m/s² (see table, last row)!

           real           measured                   estimated                    real
time   pos     acc       pos    acc        pos      acc    jerk    vel[!]       velocity
0.0   0.000  -0.000    -0.040  0.030  |  -0.300   -0.060   0.000   18.850  <-->  18.850
0.1   1.885  -0.113     1.965 -0.153  |   1.585   -0.061  -0.006   18.844  <-->  18.844
0.2   3.768  -0.227     3.778 -0.247  |   3.469   -0.066  -0.035   18.835  <-->  18.827
0.3   5.650  -0.340     5.750 -0.370  |   5.351   -0.090  -0.122   18.815  <-->  18.799
0.4   7.528  -0.452     7.358 -0.452  |   7.228   -0.152  -0.291   18.769  <-->  18.759
0.5   9.401  -0.565     9.251 -0.555  |   9.094   -0.282  -0.574   18.673  <-->  18.708
0.6   11.269 -0.677    11.309 -0.717  |   10.938  -0.518  -1.006   18.494  <-->  18.646
0.7   13.130 -0.788    13.260 -0.758  |   12.752  -0.840  -1.490   18.233  <-->  18.573
0.8   14.983 -0.899    15.043 -0.949  |   14.520  -1.286  -2.096   17.854  <-->  18.488
0.9   16.827 -1.009    16.977 -1.089  |   16.235  -1.838  -2.770   17.362  <-->  18.393
1.0   18.661 -1.118    18.831 -1.168  |   17.890  -2.477  -3.476   16.762  <-->  18.287


My matrices are here:

What is causing this addition in each timestep for jerk? Is any of my matrices wrong?

The same goes with the first solution (only 3 state model) - the acceleration isn't changing as it should.

LAST EDIT:

I've finally managed to make it work. I am not sure if there was an implementation error or wrong P&Q matrices.

• To get a useful answer you need to provide more information. Describe the system. Show us what equations you have put together. – Jim Clay Sep 3 '12 at 15:17
• Thank you for your response. I have provided additional information. – Primož Kralj Sep 3 '12 at 20:11
• Please someone help me with this - I feel like i'm loosing my mind. – Primož Kralj Sep 5 '12 at 7:56
• I don't see any obvious problem. It's likely that you have an implementation error. – Jason R Sep 5 '12 at 12:11
• @Phonon, no hard feelings. I feel like I've been on a rollercoaster though :) – Primož Kralj Sep 5 '12 at 15:58

You would not make a constant-acceleration assumption in this case. You would typically do that if you had no means of measuring the system's acceleration, but you say that it is observable for your case. The most obvious way to model this system would be using the state vector

$$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \end{array} \right ]$$

where $x_k$ is the position, $v_k$ is the velocity, and $a_k$ is the acceleration of the system at time instant $k$. Since you say that you can measure the position and acceleration of the system, the measurement vector $\mathbf{z_k}$ would be:

$$\mathbf{z_k} = \mathbf{Hx_k} + \mathbf{v_k}$$

with

$$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 \\ 0 && 0 && 1 \end{array} \right]$$

resulting in the measurement model:

$$\mathbf{z_k} = \left[ \begin{array}{c} x_k \\ a_k \end{array} \right] + \mathbf{v_k}$$

where $\mathbf{v_k}$ is the (Gaussian) noise inherent in the measurement. Now, unless you are adding some known force to the system that would affect its dynamics, then the input vector $\mathbf{u_k} = \mathbf{0}$. See this recent question for more detailed discussion of the various terms in the Kalman model, which you need to know, and which you don't.

Edit: Regarding the below comment on how to handle the state transition matrix for the acceleration term: there are a couple different ways to handle that. You could assume a zero-jerk (jerk is the time derivative of acceleration) model; this is equivalent to assuming that $a_{k+1}=a_k$. Or, you could add a fourth element to your state vector:

$$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \\ \dddot{x_k} \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \\ j_k \end{array} \right ]$$

where $j_k$ is the system's jerk, or time rate of change of acceleration, at time instant $k$. Since you aren't measuring jerk directly, you would have a measurement matrix of:

$$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 && 0\\ 0 && 0 && 1 && 0 \end{array} \right]$$

And the state transition matrix would become:

$$\mathbf{F} = \left[ \begin{array}{cccc} 1 && dt && \frac{1}{2}dt^2 && \frac{1}{6} dt^3 \\ 0 && 1 && dt && \frac{1}{2}dt^2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \end{array} \right]$$

This is a "constant-jerk" model (similar to constant-velocity or constant-acceleration models you might see in simple examples); your state transition matrix makes an implicit assumption that the jerk is constant for all values of k. This is not likely to be true. To handle that aspect of the problem, you would introduce a process noise term in the jerk component of the state transition equation.

Qualitatively, this allows you to express that you aren't sure how the jerk term is going to change from time step to time step, but that you expect the changes to be random with some Gaussian distribution. This is a tool often used to "tune" adaptive filter models like this one until you find a set of parameters that works well for your application.

• Thank you, you really lit a light in my head. But I am still not grasping what is my acceleration expecting to be in next step (if not constant) - this is important to build transition matrix A, the last row to be precise. What would you suggest? – Primož Kralj Sep 4 '12 at 13:31
• There is a whole mess with migrating questions. Part of your answer is gone. I hope mods repair this. Also, look at Addition 2 in my original question. – Primož Kralj Sep 4 '12 at 19:32
• I was able to recover the edited text and reapplied it to my answer. – Jason R Sep 5 '12 at 12:16
• Could you explain how to get process covariance for first solution (3 state variables)? I can't find a decent link online which would explain it thorough in practice. – Primož Kralj Sep 5 '12 at 16:00

I'm assuming that your state variables are as per Jason's answer. If that's the case, then I believe your problem might be your $Q$ matrix (process noise): because it is zero everywhere except for the jerk update, the only terms updating the values of the position, velocity and acceleration are the deterministic parts of the update equations (except where the stochastic nature of the jerk feeds through).

Try using a $Q$ matrix like:

$$Q = \left [ \begin{array}{cccc} 0.00001 & 0& 0& 0\\ 0 & 0.00001 & 0 & 0\\ 0 & 0 & 0.00001 & 0 \\ 0 & 0& 0& 0.001 \end{array} \right ]$$

If those diagonal elements are zero, the model is saying "I don't expect the position, velocity or acceleration to change (other than by the update equations)" --- which is clearly not the case.

• Nice explaination - but why choose 0.001, why not 2.84 for example? – Primož Kralj Sep 6 '12 at 6:09
• Thanks! The reason for choosing 0.00001 over 2.84: Well, 2.84 is VERY large for a variance. I assumed that you had a valid reasoning behind the jerk number (0.001), and that the other numbers should be significantly smaller than this. If there is no real reason for choosing the jerk number, try the same value in all diagonal elements. – Peter K. Sep 6 '12 at 11:51
• But what does this mean in practice? If I say my position sensor has 0.00001 variance (first diag. element in Q matrix) does that mean it's mistake is +- 0.00001 meters for example (I guess not)? I guess you choose smaller variance for a sensor that is more precise (position) and a bigger variance for a sensor with less precision (acceleration). – Primož Kralj Sep 6 '12 at 12:08
• If you say that the variance of a measurement is $0.00001$, then in the Kalman filter framework, you're saying that it is Gaussian distributed with a standard deviation of $\sqrt{0.00001}$. So, ~68% of the time, the measurement error magnitude will be less than $\sqrt{0.00001}$, ~95% of the time, it will be less than $2\sqrt{0.00001}$, and so on. – Jason R Sep 6 '12 at 12:11
• Thank you both, I hope this thread will help future visitors too. – Primož Kralj Sep 6 '12 at 12:30

I think the problem is in Q. Q are charging K with their values. I achieve good results when Q is preloaded with powers of delta_t.

See any article about wiener model. It has a good Q matrix.

A00 has to be multiple of delta_t power of 5 A11 delta_t power of 3 A22 delta_t power of 1 A01 multiple of delta_t power of 4

And so on

Look at