Kalman Filter with Example. C Code and Octave Script
1. Abbreviation
2. Introduction
Suppose there is a dynamic system (6) for which the state vector ‘x’ is defined, for example, the position of the object, its speed, acceleration and so on. We make inaccurate (with error) measurements of some noisy signals ‘y’, knowing which, we can calculate the value of the state vector of the system. Kalman proposed an optimal recurrent algorithm for estimating system parameters. Since the measurement/calculation is performed over a vector, the equations also have a matrix form.
The Kalman filter minimizes the square of the estimation error (10):
The filter works in two stages:
-
extrapolation (prediction) of the next state of the system x(-) based on the system model
-
measurement of noisy parameters ‘y’, after which the prediction correction x(+) is carried out
See the Figure 2-1.
Figure 2-1: Kalman Filter: Prediction – Update States
Notes:
-
In the previous article “Simple Scalar Kalman Filter. C Implementation Using the Octave GNU Tool” an example of a simple scalar Kalman Filter was considered. Here I will give an example of a matrix variant
-
Next I will describe the theory of Kalman Filter and immediately apply it to my practical example, which can be used in many applications
3. System Dynamic Model
The System Dynamic Model is described using the formula (6):
Let’s go straight to my simple practical example. Any signal can be described by the current value and the rate of signal change:
The signal speed can be changed and this change can be represented as noise (plant maneuvering noise):
Formulas (12) and (13) can be rewritten in matrix form (6):
4. Model of the data measurement
Measurement model is described (8):
Now let’s go to our example. We measure the value of our signal, but our measurement is subject to noise:
Let’s rewrite this formula in matrix form (8):
5. Kalman Filter
Knowing the System Dynamic Model (6)/(14) and the measurement channel (8)/(16), it is possible to construct a Kalman filter that minimizes the error (11) of the ‘x’ state estimation. The structure of the discrete Kalman filter is shown in Figure 5-1:
Figure 5-1: Structure of the Kalman Filter
Next I will give the Kalman filter algorithm without proof.
-
Prediction Step:
-
Kalman Gain Calculation:
-
Measurement of the
-
Calculation of the output values of the Kalman filter:
-
Increment k=k+1 and go to point 1
The separation of the Filter into two stages: “Prediction – Update after Next Measurement“ is important in control systems, since in this case it is possible to achieve a greater margin of stability. If the Kalman filter is used for example to smooth the signal then the algorithm can be simplified by excluding the Prediction State from it. In this case only the previous and subsequent state of the filter is used: x(+), P(+) without intermediate prediction variables: x(-), P(-). To obtain formulas for this simplified version, substitute (18) in (19), (21), (22) and discard the ‘(+)’ :
-
Calculation of Kalman Gain by the formula:
-
Measurement of the
-
Calculation of the output values of the Kalman filter:
-
Increment k=k+1 and go to point 1
For our example I will use the Kalman filter with the prediction step of formulas (18) – (22) to avoid complex entries:
-
Prediction Step:
-
Kalman Gain Calculation:
-
Measurement of the
-
Calculation of the output values of the Kalman filter:
-
Increment k=k+1 and go to point 1
6. Implementation of our Example
To implement the example, it is necessary to set or calculate the system parameters. As the initial value of the state vector ‘x’, we take zero values:
Then:
Now let’s choose the value of maneuvering noise. To do this, it is necessary to determine the change in the signal speed during sampling Ts. Let our signal work in the frequency band 0…100Hz and has a maximum amplitude of 1 Volt. Let’s choose Ts=0.25ms. Then we will calculate the speed change (maneuvering) for the maximum frequency 100Hz:
Using the 3σ rule for the Gaussian distribution, we can write:
Now let’s select the noise parameters measurement model:
Now it is possible to calculate the values (32):
This example is implemented in demo SW using formulas (26) – (37). See the files:
-
KalmanFilterExampleSupport.m – Octave Script
-
KalmanFilterExample.c/h – C Code
Below are showed the plots of the Kalman filter from our example:
-
Original signal value = x(1,1). See the Figure 6-1
-
Measurement Noise v. See the Figure 6-2
-
Signal Value + Noise. See the Figure 6-3
-
Output of the Kalman Filter. See the Figure 6-4
-
Kalman Filter: Kalman Gain of the Signal Value. See the Figure 6-5
-
Kalman Filter: P Estimate Value Error. See the Figure 6-6
Figure 6-1 Original input signal value = x(1,1)
Figure 6-3: Signal and Noise ‘y’ from Measurement Model
Figure 6-4: Kalman Filter: Output Value Estimation: KalmanOut(1,1)
Figure 6-5: Kalman Filter: Kalman Gain of the Signal Value: K_Gain(1,1)
Figure 6-6: Kalman Filter: P Estimate Value Error: P_Out(1,1)
Note
-
After some operation time the filter goes into steady-state mode and the values of K and P take constant values:
The K value indicates that at a given noise level and signal correlation, the confidence in the measured signal: ‘y’ is 25%, the rest 75% is taken from the filter’s prehistory: predicted x(-). In this case the power of the prediction error value (P) is 0.01 V^2
7. Octave GNU Script KalmanFilterExampleSupport.m
The Octave script supports all plots of the article, generates the test signal file / array for the C code and calculates the Kalman filter.
Script Functions:
% Float Test Signal Table to File
% Support C-Code
% Input parameters:
% Amplitude in Volt
% Variance_o – observation noise power
% Frequency – 2*pi*f*Ts
% FileNameString – output file name
% Output:
% txt file with the signal data
% testSignal = Amplitude*cos(Frequency*n) + observationWhiteNoise
function testSignal = FloatTestSignal2file(Amplitude, Variance_o, Frequency, FileNameString)
% Kalman Filter Example
% Input parameters:
% signalIn – Input Signal
% Phi_matrix – transition matrix
% Q_matrix – covariance matrix of the plant noise
% H_matrix – measurement sensitivity matrix
% R_matrix – covariance matrix of the observation noise
% P0 – start value: covariance matrix of the estimate error
% x0 – start value of the x state vector
% Output:
% signalOut – output of the Kalman Filter
% KalmanGain – Kalman Gain
% P_Covariance – P covariance: Estimate Error
function [signalOut, KalmanGain, P_Covariance] = KalmanFilterExample(signalIn, Phi_matrix, Q_matrix, H_matrix, R_matrix, P0, x0)
8. C Code file KalmanFilterExample.c/h
Function:
/*
Scalar Kalman Filter
Input
float input – input measured signal
kalman_filter_data_s* kalman_data – Kalman filter data
Return
float x[0] estimate value
*/
float KalmanFilterExample(float input, kalman_filter_data_s* kalman_data)
9. Download the KalmanFilterExampleSupport.m and KalmanFilterExample.c/h
You can download the files:
-
KalmanFilterExampleSupport.m
-
KalmanFilterExample.c/h