kalman-cpp
Implementation of Kalman Filter and extended Kalman filter in C++
kalman-cpp

Kalman filter for a linear system

Definition

A linear system is described by:

\[x_k = Ax_{k-1} + Bu_{k-1} + v_{k-1}\]

\[z_k = Hx_k + w_k\]

where:
$v$ is the process noise (Gaussian with covariance Q)
$w$ is the measurement noise (Gaussian with covariance R)
$A$ is the system matrix
$B$ is the input matrix
$H$ is the output matrix
$x$ is the state vector
$z$ is the output vector
$u$ is the input vector

The noise covariance matrices must follow these conditions:

\[Q = Q^T\]

\[R = R^T\]

Example

An example provided in main1.cpp solves the car voltmeter problem using this library. The system model for this problem is governed by:

\[x_k = 12 + v_{k-1}\]

\[z_k = x_k + w_ḳ\]

Therefore, we can summarize: $A = 0$, $B = 1$, $H = 1$, $u = 12$, $x_0 = 12$, $Q=4$, and $R=4$. These values are the put into the following lines of codes.

mat A(1,1), B(1,1), H(1,1), Q(1,1), R(1,1);
colvec u(1);
A << 0;
B << 1;
Q << 4;
H << 1;
R << 4;
u << 12.0;

The first line of the code above is used to create all necessary matrices with correct dimensions. All matrix operations use the Armadillo library. Please refer to the Armadillo documentation on how to create a matrix and set its element values. The next step is to create an instance of class KF and initialize it with the newly created $A$, $B$, $Q$, $H$, and $R$.

KF kalman;
kalman.InitSystem(A, B, H, Q, R);

The last step is to run the Kalman iteratively as shown in the codes below.

for (int k = 0; k < 100; k++) {
kalman.Kalmanf(u);
/* The rest of the code ... */
}
ex1.png

Extended Kalman filter for a nonlinear system

Definition

A non-linear system is described by:

\[x_k = f(x_{k-1}, u_{k-1}) + v_{k-1}\]

\[z_k = h(x_k) + w_k\]

where:
$f$ is the dynamic model of the system
$h$ is the measurement model of the system
$v$ is the process noise (Gaussian with covariance Q)
$w$ is the measurement noise (Gaussian with covariance R)
$x$ is the state vector
$z$ is the output vector
$u$ is the input vector

The noise covariance matrices must follow these conditions:

\[Q = Q^T\]

\[R = R^T\]

Example

Here, main4.cpp is taken as an example which is adapted from this MATLAB Central page. The nonlinear system is described as:

\[ f = \begin{bmatrix} sin(x_2(k-1))(k-1) \\ x_2(k-1) \end{bmatrix} \]

\[ h = \begin{bmatrix} x_1(k) \\ x_2(k) \end{bmatrix} \]

\[ x_0 = \begin{bmatrix} 0 \\ \frac{1 \pi}{500} \end{bmatrix}\]

Class EKF needs to be derived to allow flexible implementation of the dynamic model and the measurement model of the system .

class MyEKF: public EKF
{
public:
virtual colvec f(const colvec& x, const colvec& u) {
colvec xk(nOutputs_);
xk(0) = sin(x(1) * u(0));
xk(1) = x(1);
return xk;
}
virtual colvec h(const colvec& x) {
colvec zk(nOutputs_);
zk(0) = x(0);
zk(1) = x(1);
return zk;
}
};

The following steps are similiar with the previous example as in the linear Kalman filter. We start by defining the number of states and outputs, then followed by creating matrix Q and R. An instance of class EKF is created afterward and the states are initialized.

int main(int argc, char** argv)
{
/*
* Log the result into a tab delimitted file, later we can open
* it with Matlab. Use: plot_data4.m to plot the results.
*/
ofstream log_file;
log_file.open("log_file4.txt");
int n_states = 2;
int n_outputs = 2;
mat Q(2, 2);
mat R(2, 2);
Q << 0.001 << 0 << endr
<< 0 << 0 << endr;
R << 0.1 << 0 << endr
<< 0 << 0.01 << endr;
colvec x0(2);
x0 << 0 << 1 * M_PI / 500;
colvec u(1);
MyEKF myekf;
myekf.InitSystem(n_states, n_outputs, Q, R);
myekf.InitSystemState(x0);
for (int k = 0; k < 1000; k ++) {
u(0) = k;
myekf.EKalmanf(u);
colvec *x = myekf.GetCurrentState();
colvec *x_m = myekf.GetCurrentEstimatedState();
colvec *z = myekf.GetCurrentOutput();
log_file << k
<< '\t' << z->at(0,0) << '\t' << x->at(0,0) << '\t' << x_m->at(0,0)
<< '\t' << z->at(1,0) << '\t' << x->at(1,0) << '\t' << x_m->at(1,0)
<< '\n';
}
log_file.close();
return 0;
}
ex4.png

Practical application: Using Kalman filter for smoothing noisy measurement data

The examples we have so far are theoritical. Very often what we would like to do is simply to reduce noise from preacquired measurement data. There are several reason why we want to use Kalman filter. For example, the noise has very wide spectrum, thus, using frequency based filter hurts the data.

In principal, there are 2 scenarios of using the Kalman filter. The first scenario is by first simulating the system as shown in the figure below. In this scenario, we only need to supply $u_k$ to the Kalman filter function. The Kalman filter will give us 4 outputs: $x_k$, $z_k$, $\hat{x}_k$, and $\hat{z}_k$. $x_k$ and $z_k$ are called the true system states and the true system outputs, respectvely. They are noisy. $\hat{x}_k$, and $\hat{z}_k$ are called the estimated system states and the estimated system outputs, respectively. They are filtered.

The function prototype for this scenario can be written as: [ $x_k$, $z_k$, $\hat{x}_k$, $\hat{z}_k$] = function kalmanf( $u_k$)

Kalman_concept1.jpg

The second scenario is used when the measurements are availble. Thus, simulating the system becomes unnecessary. In this scenario, we need to supply $z_k$ and $u_k$ to the kalman filter function. The Kalman filter will give us 2 outputs: $\hat{x}_k$ (the estimated system sates) and $\hat{z}_k$ (the estimated system outputs).

The function prototype for this scenario can be written as: [ $\hat{x}_k$, $\hat{z}_k$] = function kalmanf( $z_k$, $u_k$)

Kalman_concept2.jpg

The second scenario is useful for smoothing noisy measurment data. However, both scenarios are availabe in this library.

Example

As an example, we will take the problem in main1.cpp. This time, however, we assume we already had several noisy data points from measurements. Therfore, we will not need the Kalman procedure to simulate the system.

The system model can be formulated into:

\[x_k = x_{k-1} + v_{k-1}\]

\[z_k = x_k + w_ḳ\]

We then generate random number as the prerecorded noisy data (z_measurement), as the following:

// Assume we have a noisy signal that is acquired from a measurement tool
double w = 2; // Stdev of the noise, in reality we don't know this
colvec z_measurement(1);
z_measurement.randn(1);
z_measurement = z_measurement * w + 12.0;

After than, z_measurement is then sent to the Kalman filter function, as the following:

// Put the z_measurment to the Kalman filter
kalman.Kalmanf(z_measurement, u);
colvec *z_m = kalman.GetCurrentEstimatedOutput();
colvec *x_m = kalman.GetCurrentEstimatedState();

The kalman.Kalmanf(z_measurement, u) function above has different parameters with what we can find in main1.cpp. Since z_measurement is available, we then send it as a parameter for the kalmanf function. The processes above are done in several iterations according to the number of the availabe data points. The final codes will look like as the following:

int main(int argc, char** argv)
{
/*
* Define the system and initialize the Kalman filter
*/
mat A(1,1), B(1,1), H(1,1), Q(1,1), R(1,1);
colvec u(1);
colvec x0(1);
A << 1;
B << 1;
Q << 0.01; // Heuristic tuning parameter
H << 1;
R << 1; // Heuristic tuning parameter
u << 0;
KF kalman;
kalman.InitSystem(A, B, H, Q, R);
/*
* For plotting with matplotlibcpp
*/
int N = 500;
std::vector<double> k_plot(N), noisy_plot(500), smooth_plot(N);
for (int k = 0; k < N; k++) {
// Assume we have a noisy signal that is acquired from a measurement tool
double w = 2; // Stdev of the noise, in reality we don't know this
colvec z_measurement(1);
z_measurement.randn(1);
z_measurement = z_measurement * w + 12.0;
// Put the z_measurment to the Kalman filter
kalman.Kalmanf(z_measurement, u);
colvec *z_m = kalman.GetCurrentEstimatedOutput();
colvec *x_m = kalman.GetCurrentEstimatedState();
k_plot.at(k) = k;
noisy_plot.at(k) = z_measurement.at(0,0);
smooth_plot.at(k) = z_m->at(0,0);
}
}

The values that are given to Q and R are defined heuristically since we do not know the actual variances of the noise of the data.

ex7.png