2
$\begingroup$

Below are the equations for state estimation using Kalman Filter. Here are the first few equations, and the rest follows in a link below:

$$ \newcommand{\blue}{\color{blue}} \newcommand{\grey}{\color{darkgrey}} \newcommand{\red}{\color{red}} \newcommand{\orange}{\color{orange}} \begin{align} & \left. \begin{array}{ll} \text{State Prediction} & \grey{X_{predicted}}=A\grey{X_{n-1}}+B\blue{u_n}\\ \text{Covariance Prediction} &\grey{P_{predicted}}=A\grey{P_{n-1}}A^{\rm T}+Q \end{array} \right\}\red{\text{Prediction Step}} \\ \quad\\ & \left. \begin{array}{ll} \text{Innovation} & \grey{y}=\blue{z_n}-H\grey{X_{predicted}}\\ \text{Innovation Covariance} &\grey{S}=H\grey{P_{predicted}}H^{\rm T}+R\\ \text{Kalman Gain} & \grey{K}=\grey{P_{predicted}}H^{\rm T}\grey{S^{\rm -1}}\\ \text{State Update} & \orange{X_n}=\grey{X_{predicted}}+\grey{Ky}\\ \text{Covariance Update} & \orange{P_n}=(\grey{I}-\grey{K}H)\grey{P_{predicted}} \end{array} \right\}\red{\text{Measurement Update}} \end{align} $$

Kalman Filter Equations

So, in the prediction stage, the filter predicts the states and the co-variance matrix of the states using a linear model.

Then in the measurement or sensor stage, the filter takes the sensor readings into accounts and updates the states and it's covariance matrix.

My question is, do I need to run the Kalman Filter just once or in a loop? And how do I be sure that the states estimated (X) from the Kalman Filter have approached the real or actual values? Shouldn't we need to take a look into the co-variance matrix P?

The reason I am asking this question is, I wrote a code for tracking objects using Kalman Filter. After some iterations using the same sensor reading, I found out that the covariance matrix after the measurement update isn't updating anymore i.e. the covariance matrix, P, is retaining the same value over the next iterations onward. Is this the indication that the Kalman Filter has done the best it could?

$\endgroup$
  • $\begingroup$ Since you are new to this site, I suppose you do not know how to type math symbols here using MathJaX. Perhaps someone would be kind enough to edit the equations into your post, to get you started. $\endgroup$ – String Nov 20 '16 at 15:47
  • $\begingroup$ I have put in the first few to get your started - can you take it from there? If you click edit, you should be able to see, what I have done. $\endgroup$ – String Nov 20 '16 at 15:53
  • $\begingroup$ Thanks @String, for the edit. Yes, I didn't know MathJaX. I copied your code and updated the equations. I think I got it but there are two sets of equations now, and I don't know how to combine them into a single set. Thanks for your help though. $\endgroup$ – benjamin Nov 20 '16 at 17:55
  • $\begingroup$ Is my last edit what you intended? I used \begin{align} ... \end{align} which is an environment that then aligns at the inserted &-symbols of each line. Look up examples by searching for $\LaTeX$ and 'align'. $\endgroup$ – String Nov 20 '16 at 20:16
  • $\begingroup$ Yes, that's what I wanted. Thanks! $\endgroup$ – benjamin Nov 20 '16 at 21:13

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.