# Velocity estimation knowing acceleration and displacement from measurements using Kalman filter

I am having a hard time in trying to use a Kalman filter to obtain velocity from acceleration and position measurements. I think the main reason is that I am not familiar with Kalman filters (I had only a couple of lectures about it and I implemented it only once).

To start I had a look to some questions here and also on a paper I found... and now I am confused.

This is what the paper says: the noisy acceleration measurement is the driving input and the measurements are the measured noisy states.

The equations for the measurement dynamics can be written as:

$\begin{bmatrix}\dot{x} \\ \ddot{x}\end{bmatrix} = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}\begin{bmatrix} x \\ \dot{x}\end{bmatrix}+ \begin{bmatrix}0 \\ 1\end{bmatrix} \ddot{x_a} + \begin{bmatrix}0 \\1\end{bmatrix}\eta_a$

$y = x_a = [1 \ 0] \begin{bmatrix} x \\ \dot{x} \end{bmatrix} + \eta_d$

The $\eta$ terms take into account the bias and noise errors.

In matrix form:

$\dot{x} = Ax+Bu+v$

$y = Cx+w$

$v$ and $w$ describe the noise in the accelerometer and displacement.

The feedback estimation formulation for the state estimates \hat{x} is

$\dot{\hat{x}} = A \hat{x} + B u - L (y-C \hat{x})$

where $L = [l_1 \ l_2]^T$ represents the desired feedbagk gains

Then it keeps going talking about the dynamics of the error signal $\epsilon = x-\hat{x}$ and analysing the steady-state error $\epsilon_{SS} = - [A-LC]^{-1}(v+Lw)$ it comes to the conclusion that the state vector needs to be augmented:

$\begin{bmatrix}\dot{x} \\ \ddot{x} \\ \dot{\eta}_a \end{bmatrix} = \begin{bmatrix}0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \\ \eta_a \end{bmatrix} + \begin{bmatrix}0 \\ 1 \\ 0 \end{bmatrix} \ddot{x}_a + \begin{bmatrix}0 \\ 0 \\ 1 \end{bmatrix} \dot{\eta}_a$

$y = [1 \ 0 \ 0] \begin{bmatrix} x \\ \dot{x} \\ \eta_a \end{bmatrix} + \eta_d$

What I do not understand is where the velocity (which I need to estimate) comes into play in this formulation.

Therefore I had a look at this question Estimating velocity from known position and acceleration, but I do not get how to write the state matrix $A$.

In fact, if the state vector is $[x \ \dot{x} \ \ddot{x}]^T$ (as said in the answer) then I should write

$\begin{bmatrix} \dot{x} \\ \ddot{x} \\ \dddot{x} \end{bmatrix}= \begin{bmatrix} & & \\ & & \\ & & \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \\ \ddot{x} \end{bmatrix} + B u$

First question, what are the equations I have to use to fill matrix $A$?

What are $B$ and $u$ in this case?

Why 2 such different approaches to model this problem?

Then, once I have found the correct formulation of the problem I have to compute the matrix $L$ and this can be done using the Matlab function kalman (knowing the covariance matrices of $v$ and $w$). After that do I have to integrate the equation of the observer $\dot{\hat{x}} = A \hat{x} + B u - L (y-C \hat{x})$ to get my estimated state?

• I think you're making this too complicated. I would read through the other question again, as it is aiming to do exactly what you want. It's just adapted from the very simple yet easy-to-understand example at the Wikipedia article on Kalman filters. I don't think you want to treat the acceleration as a noisy input if it's something that you measure; just treat it as part of the state vector, with a corresponding entry in the measurement matrix. – Jason R Jan 29 '15 at 13:01
• Just one question, since my measurements represents displacement and acceleration of a non-linear system, do I have to modify the formulation of the filter or it does not matter since the equations of the filter are still linear? – Rhei Feb 26 '15 at 19:17