## Kategoriearchiv

Du betrachtest das Archiv der Kategorie Kalman Filter.

• ## Regular Kalman Filter (almost) super-quick Reference

To make long things short, here’s the complete Matlab code.

% State estimations
x       state vector                  (M x 1)
A       state transition matrix       (M x M)
P       state covariance matrix       (M x M)

% Input / control data
u       input vector                  (N x 1)
B       input transition matrix       (M x N)
Q       input noise covariance matrix (N x N)

% Observations
z       observation vector            (Z x 1)
H       state-to-observation matrix   (Z x M)
R       observation noise covariance  (Z x Z)

% tuning
lambda  tuning parameter              (scalar)

function [x, P] = kf_predict (x, A, P, lambda , u, B, Q)
x = A*x + B*u; % a priori state prediction
P = A*P*A' * 1/( lambda ^2) + B*Q*B'; % a priori covariance
end

function [x, P] = kf_update (x, z, P, H, R)
y = z - H*x; % measurement residuals ("innovation")
S = H*P*H' + R; % residual (innovation) covariance
K = P*H' / S; % Kalman gain
x = x + K*y; % a posteriori state prediction
P = (eye(size(P)) - K*H)*P; % a posteriori covariance matrix
end

• ## Kalman filter: Modeling integration drift

One interesting observation when working with the standard model for constant acceleration in the Kalman filter is that the results tend to drift over time, even if the input to the system is zero and unbiased. I stumbled across this recently when integrating angular velocities measured using a gyroscope. Obviously, calibrating the gyroscope is the first step to take, but even then, after a while, the estimation will be off.

So the differential equations describing motion with constant acceleration are given as

\begin{align} x(t) &= x_0 + v(t)\,\mathrm dt + \frac{1}{2}a(t)\,\mathrm dt^2 \\ v(t) &= v_0 + a(t)\,\mathrm dt \\ a(t) &= \mathrm{const} \end{align}

The continuous-time state-space representation of which being

\begin{align} \dot{\vec{x}}(t) = \underbrace{\begin{bmatrix} 0 & \mathrm dt & 0.5\,\mathrm dt^2 \\ 0 & 0 & \mathrm dt \\ 0 & 0 & 0 \end{bmatrix}}_{\underline{A}} \cdot \underbrace{\begin{bmatrix} x \\ v \\ a \end{bmatrix}}_{\vec{x}} \end{align}

where the state vector $\vec{x}$ would be initialized with $\left[x_0, v_0, a_0\right]^T$. Modeled as a discrete-time system, we then have

\begin{align} \vec{x}_{k+1} = \begin{bmatrix} 1 & T & 0.5\,\mathrm T^2 \\ 0 & 1 & \mathrm T \\ 0 & 0 & 1 \end{bmatrix}_k \cdot \begin{bmatrix} x \\ v \\ a \end{bmatrix}_k \end{align}

with $T$ being the time constant.

Now due to machine precision and rounding issues we’ll end up with an error in every time step that is propagated from the acceleration to the position through the double integration. Even if we could rule out these problems, we still would have to handle the case of drift caused by noise.

According to Position Recovery from Accelerometric Sensors (Antonio Filieri, Rossella Melchiotti) and Error Reduction Techniques for a MEMS Accelerometer-based Digital Input Device (Tsang Chi Chiu), the integration drift can be modeled as process noise in the Kalman filter.

Tsang (appendix B, eq. 7) shows that the drift noise is given as

\begin{align} \underline{Q}_a = \begin{bmatrix} \frac{1}{20} q_a \,T^5 & \frac{1}{8} q_a \,T^4 & \frac{1}{6} q_a \,T^3 \\ \frac{1}{8} q_a \,T^4 & \frac{1}{3} q_a \,T^3 & \frac{1}{2} q_a \,T^2 \\ \frac{1}{6} q_a \,T^3 & \frac{1}{2} q_a \,T^2 & q_a \,T \end{bmatrix} \end{align}

with $q_a$ being the acceleration process noise (note that Tsang models this as $q_c$ in continuous-time).

• ## libfixkalman: Fixed-Point Kalman Filter in C

In need for a Kalman filter on an embedded system I was looking for a linear algebra library. It turned out that there are quite a bunch of libraries written in C++, mostly template based, yet nothing lean and mean written in ANSI C. I ended up porting parts of EJML to C and picked only what I needed for the Kalman filter (see the result, kalman-clib, if you are interested). The result was a working, relatively fast library using floats. Or doubles, if the user prefers.

However, a big problem is that many embedded targets (say, ARM Cortex-M0/3) don’t sport a floating-point unit, so the search went on for a fixed-point linear algebra library. Thanks to a hint on StackOverflow it ended at libfixmath, which implements Q16 numbers (i.e. 16.16 on 32bit integer) and is released under an MIT license.

Luckily, some other guy created a linear algebra library around it, called libfixmatrix (MIT, too). I started to port my Kalman library to libfixmatrix and, voilà, libfixkalman was born (you can find it in the libfixkalman GitHub repository).