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?

  • $\begingroup$ 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. $\endgroup$
    – Jason R
    Commented Jan 29, 2015 at 13:01
  • $\begingroup$ 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? $\endgroup$
    – Rhei
    Commented Feb 26, 2015 at 19:17


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.