Electronic – Kalman filter on linear acceleration for distance

In this there are 2 matrices Q and R for noise modelling and they have set them such that displacement error is minimized.

In the Kalman Filter, Q and R (the process noise covariance and the measurement noise covariance, respectively) are not really tunable in the sense that the quoted text implies. They are characterizations of the system which you can measure (or estimate). When the KF is described as optimal it means that it minimizes the variance of its output. It does this by using a balance of the known/predicted state and the newly measured state according to how much noise it thinks there is in each.

The filter will obviously have a different result if you change R, but that implies that you can change the behavior of the sensors (in this case, your accelerometer). The values in R come directly out of the datasheet for your accelerometer for the most part. Where it talks about "RMS noise" (root mean squared) that's the standard deviation (sigma) of the noise in the chip's output. Variance in R is sigma^2 (or just "mean squared") so you can just square that value and put it on the diagonal of R. Sometimes the datasheet will specify it in terms of something else (e.g. noise per sqrt(Hz)) and you will have to do a calculation first based on how you have configured the chip.

The measurement noise covariance R often has no covarying terms (so all off-diagonal terms are 0) because the noise of all of the sensors is independent. If you had an array of similar sensors (e.g. temperature probes with a breeze wafting across) which were subject to a common disturbance, then you might have covariance. You can always take long measurements from your sensors at idle and directly compute R by hand or with readily available tools like Excel or numpy.cov().

The process noise covariance Q is more tricky. This is somewhat of a tunable parameter, but you must keep in mind what it means and what you are representing by setting it. Imagine your system does a predict/update step 100 times a second. Between updates, when you are not looking at your system, it can change. If it can't change (e.g. you measure the compass heading of a sensor anchored in concrete) then your Q can be 0, which reflects the idea that your belief only gets better with time and repeated averaging. If it can change (in your case, move) then you use Q to tell the filter how much to erode its belief in position over the 10ms when you were sleeping.

The reason that we use covariance matrices and not co-standard-deviation matricies is that variance adds when you add two independent random variables. That's why we can form Q and add it in during the predict step each time. The filter has established some error variance for the state you're trying to estimate, and now something unknown has happened to it which the KF assumes is gaussian noise with covariance Q.

Let's say you think that during the 10ms you are not looking, your tracked object will be within 1mm of where it was last time 68% of the time. This is equivalent to saying that the sigma (or standard deviation) of your position is 1mm. That means the associated variance is (1mm)^2 (mind your decimal places if your units are meters!). If you are wrong about your assumption then one of two things will happen:

• It actually moves more than that: The filter will lag the position because it will believe its estimate more than the sensor telling it about the larger move.
• It actually moves less than that: Your filter will track with minimal lag, but more noise than necessary will be admitted because more of the measurement is used than necessary.

So you can tune it by moving Q up and down, but the process has some true value for Q which you can measure (for position you would not find the covariance of the position, but the covariance of the deltas in position).

When you represent multiple time derivatives of position (velocity, acceleration, jerk) then they will obviously have co-varying terms (i.e. nonzero values off the diagonal of Q) because you can't have unexpected movement without unexpected velocity and so on. For a position-velocity model it looks like this:

[ 1/3dt^3  1/2dt^2 ]
[ 1/2dt^2    dt    ] * q

Where q is your velocity variance and dt is the timestep of your filter relative to the units of q. If you measure it yourself at your own timestep then dt=1 because q is already in (units/timestep)^2.

For a position-velocity-acceleration model it looks like this:

[ 1/20dt^5 1/8dt^4 1/6dt^3 ]
[  1/8dt^4 1/3dt^3 1/2dt^2 ] * q
[  1/6dt^3 1/2dt^2   dt    ]

And now q is acceleration covariance. Both matrices taken from Estimation with Applications to Tracking and Navigation by Bar-Shalom et. al.

Just like R, you can measure Q by measuring known movements (perhaps with some other sensor, or by looking at a video recording of the process and measuring speed).

And finally I should point out that if F, H, Q and R are all constant (which they may not be, e.g. if your timestep varies, or if your object is rotating and you must rotate the accelerometer terms around) then the value of P converges to some fixed value and stays there. The computation of P does not depend at all on your state x or your measurements z, so you can just run the filter for P and figure out what the noise covariance of your filter's output will be, even with no data.