Kategoriearchiv

Du betrachtest das Archiv der Kategorie Kalman Filter.

• Regular Kalman Filter (almost) super-quick Reference

To make long things short, here’s the com­plete Mat­lab 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 inter­est­ing obser­va­tion when work­ing with the stan­dard mod­el for con­stant accel­er­a­tion in the Kalman fil­ter is that the results tend to drift over time, even if the input to the sys­tem is zero and unbi­ased. I stum­bled across this recent­ly when inte­grat­ing angu­lar veloc­i­ties mea­sured using a gyro­scope. Obvi­ous­ly, cal­i­brat­ing the gyro­scope is the first step to take, but even then, after a while, the esti­ma­tion will be off.

So the dif­fer­en­tial equa­tions describ­ing motion with con­stant accel­er­a­tion are giv­en 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 con­tin­u­ous-time state-space rep­re­sen­ta­tion 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 vec­tor $\vec{x}$ would be ini­tial­ized with $\left[x_0, v_0, a_0\right]^T$ . Mod­eled as a dis­crete-time sys­tem, 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 con­stant.

Now due to machine pre­ci­sion and round­ing issues we’ll end up with an error in every time step that is prop­a­gat­ed from the accel­er­a­tion to the posi­tion through the dou­ble inte­gra­tion. Even if we could rule out these prob­lems, we still would have to han­dle the case of drift caused by noise.

Accord­ing to Posi­tion Recov­ery from Accelero­met­ric Sen­sors (Anto­nio Fil­ieri, Rossel­la Mel­chiot­ti) and Error Reduc­tion Tech­niques for a MEMS Accelerom­e­ter-based Dig­i­tal Input Device (Tsang Chi Chiu), the inte­gra­tion drift can be mod­eled as process noise in the Kalman fil­ter.

Tsang (appen­dix B, eq. 7) shows that the drift noise is giv­en 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 accel­er­a­tion process noise (note that Tsang mod­els this as $q_c$ in con­tin­u­ous-time).

• libfixkalman: Fixed-Point Kalman Filter in C

In need for a Kalman fil­ter on an embed­ded sys­tem I was look­ing for a lin­ear alge­bra library. It turned out that there are quite a bunch of libraries writ­ten in C++, most­ly tem­plate based, yet noth­ing lean and mean writ­ten in ANSI C. I end­ed up port­ing parts of EJML to C and picked only what I need­ed for the Kalman fil­ter (see the result, kalman-clib, if you are inter­est­ed). The result was a work­ing, rel­a­tive­ly fast library using floats. Or dou­bles, if the user prefers.

How­ev­er, a big prob­lem is that many embed­ded tar­gets (say, ARM Cor­tex-M0/3) don’t sport a float­ing-point unit, so the search went on for a fixed-point lin­ear alge­bra library. Thanks to a hint on Stack­Over­flow it end­ed at lib­fix­math, which imple­ments Q16 num­bers (i.e. 16.16 on 32bit inte­ger) and is released under an MIT license.

Luck­i­ly, some oth­er guy cre­at­ed a lin­ear alge­bra library around it, called lib­fix­ma­trix (MIT, too). I start­ed to port my Kalman library to lib­fix­ma­trix and, voilà, lib­fixkalman was born (you can find it in the lib­fixkalman GitHub repos­i­to­ry).