Simple Scalar Kalman Filter. C Implementation Using the Octave GNU Tool
1. Abbreviation
2. Introduction
In this article I will start looking at adaptive filtering. The received signal we get often in noise and / or in a distorted form. An adaptive filter is a device that adjusts its parameters during operation in order to correctly select and correctly recognize the signal. Rudolf Kalman developed his filter in 1960. During this time many modifications of this filter were created.
Let there be a dynamic system for which the ‘x’ state vector is defined for example: the position of the object, its speed, acceleration and so on. We make inaccurate (with an error) measurements of some noisy ‘y’ signals, knowing which, we can calculate / estimate the value of the ‘x’ state vector. Kalman proposed an optimal recurrent algorithm for estimating the system state. Since the measurement / calculation is performed over a vector the equations also have a matrix form.
Here we will consider only the scalar Kalman filter. In this case we will measure only one ‘y’ signal and calculate / estimate only one ‘x’ value of the system state. And all our equations will be scalar (not matrix). This will allow you to quickly understand Kalman filtering and easily use this method even in simple applications.
The Kalman filter minimizes the square of the estimation error (5):
The filter works in two stages:
-
extrapolation or prediction of the next ‘x’ state based on the system model and the measurement / estimate results from the previous step
-
next measurement of ‘y’ parameters after which the ‘x’ prediction is corrected
See the Figure 2-1.
Figure 2-1: Kalman Filter: Prediction – Update States
Notes:
-
The prediction step is important in control systems. In this case we can get an additional phase margin in the feedback circuit, which ensures greater stability of the system
-
In my article I will use the scalar model described in [1]
3. Autoregressive (AR) Model of ‘x’ state (signal) Generation
To describe the Kalman filter, it is convenient to represent the ‘x’ state (signal) using the autoregressive model (AR). The AR model of the N-th order: AR (N) is defined by:
We will use the first-order model AR(1) and const = 0 for the scalar Kalman filter. Then (7) in this case will be rewritten as:
This model is stable at values of φ:
I will use the values of φ:
Formulas (9) and (11) define the first order IIR Low Pass Filter through which white noise is passed. See the Figure 3-1:
Figure 3-1: AR(1) Signal Model
AR(1) Model Notes:
-
White noise is passed through the Low Pass IIR Filter (1st order) , while the gain coefficient of this filter is:
Considering (11):
-
At the output of the Low Pass Filter we have a red stationary noise with the parameters:
-
The formula (15) can be interpreted as the ‘x’ signal power
-
Any message is more informative if the received signal is unknown in advance and is not predictable, so the representation of the signal in the form of noise is convenient for use in dynamic models
4. Model of the ‘y’ signal Measurement
To build a Kalman filter, we need to define a measurement model. We will use the measurement model shown in Figure 4-1. In this case it is necessary to determine the ‘x’ signal, but in reality we can measure only the value of the ‘y’ signal:
Figure 4-1: Measurement Model
5. Kalman Filter
Knowing the signal model (Figure 3-1) and the measurement channel (Figure 4-1), we can build a Kalman filter that minimizes the error of ‘x’ signal (state) estimation. The structure of the discrete Kalman filter is shown in Figure 5-1:
Figure 5-1: Structure of the Scalar Kalman Filter
Next I will give the scalar Kalman filter algorithm without proof.
-
Prediction Step:
-
Kalman Gain Calculation:
-
Measurement the
-
Update after Measurement Step. Calculation of the filter output values:
-
Increment k=k+1 and go to point 1
The separation of the Filter into two stages: Prediction step and Update after Next Measurement step 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 measured signal, then the algorithm can be simplified by excluding the Prediction Step 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 get formulas for this variant, we substitute (20) in (21), (23), (24):
Thus a simplified version of the scalar Kalman filter (without prediction state) is calculated using the formulas (25) – (27):
-
Calculation of Kalman Gain by the formula:
-
Measurement the
-
Calculation of the filter output values:
-
Increment k=k+1 and go to point 1
Notes:
-
Let h = 1, then (27) can be rewritten:
The (28) can be interpreted as weighting filter: x(n) = Ky(n) + (1 – K)x(n-1) (29). The measured ‘y’ signal will be taken with wight K and previous history ‘x’ value with the weight (1 – K). If the input ‘y’ signal is not trusted then the value K shall be taken lower value. The Kalman filter calculates the optimal K gain depending on the level of interference in the system.
About the simple IIR filter (29) and (9) you can read in the article: “Simple Low Pass IIR Filter Implementation Using the C Language“
-
You can see a strict proof of formulas (25) – (27) in [1]
6. Example
Let’s consider a sinusoidal signal as an example:
To construct a Kalman filter, it is necessary to represent this signal (30) in the form of an AR(1) model. See the Figure 3-1. We shall to determine the parameters:
It is known that the power of the sinusoidal signal (30):
To determine φ, we use the formula (16a) for n=1:
Note: Next we will use the well-known formula:
Substituting (30) and (31) into (32) and using (33), as well as moving from the ensemble average to the time average:
Then for a sinusoidal signal we get:
Let’s determine the noise power using (15) and (31):
Formulas (31), (34), (35) determine the AR(1) model for the sinusoidal signal (30). Let the signal (30) have the parameters:
N = 100 and Um = 100 (36)
Now we need to set the parameters of our measurement model. See the Figure 4-1. Let’s take as an example:
It is obvious that the Signal-To-Noise Ratio (SNR) for the given example:
The example ( using formulas (25) – (27) ) is implemented in demo SW. See the files:
-
SimpleKalmanFilterSupport.m – Octave Script
-
SimpleKalmanFilter.c/h – C Code
The graphs of the scalar Kalman filter for our example are shown below:
-
Original ‘x’ signal. See the Figure 6-1
-
‘v’ noise from the measurement model. See the Figure 6-2
-
Signal + Noise. See the Figure 6-3
-
Output of the Kalman Filter. See the Figure 6-4
-
Kalman Filter: K Gain. See the Figure 6-5
-
Kalman Filter: P estimation. See the Figure 6-6
Figure 6-1 Original ‘x’ signal
Figure 6-3: Signal and Noise from Measurement Model
Figure 6-5: Kalman Filter: Kalman Gain
Figure 6-6: Kalman Filter: P estimation
Notes
-
In the steady-state mode of the filter operation the values of K and P take constant values. For our example:
These values indicate that at a given noise level and signal correlation, the confidence in the measured signal (K) is 19%, the rest (81%) is taken from the filter’s history. At the same time the power of the prediction error (P) is 84.7 -
The calculation of φ by the formula (32) and (34) for a sine wave is an approximation of the AR(1) model, which in this example gives a good result in the work of the Scalar Kalman filter. See the Figure 6-4
7. Octave GNU Script SimpleKalmanFilterSupport.m
The Octave script supports all plots of the article, generates the test signal file / array for the C code and calculates the Scalar Kalman filter.
Script Functions:
% Float Test Signal Table to File
% Support C-Code
% Input parameters:
% Variance_w – noise power for the AR(1) signal model
% Variance_v – disturb noise power
% phiValue – AR(1) model parameter
% hValue – measurement model
% signalLength – signal length
% FileNameString – output file name
% Output:
% txt file with the signal data
% testSignal = Amplitude*cos(2*pi*n/signalLength) + whiteNoise
function testSignal = FloatTestSignal2file(Variance_w, Variance_v, phiValue, hValue, periodLength, FileNameString)
% Scalar Kalman Filter
% Input parameters:
% signalIn – Input Signal
% Variance_w – noise power for the AR(1) signal model
% Variance_v – disturb noise power
% phiValue – AR(1) model parameter
% hValue – measurement model
% P0 – start value of the estimate error
% x0 – start value of the x
% Output:
% signalOut – output of the Kalman Filter
% KalmanGain – Kalman Gain
% P_Variance – P estimate error
function [signalOut, KalmanGain, P_Variance] = ScalarKalmanFilter(signalIn, Variance_w, Variance_v, phiValue, hValue, P0, x0)
8. C Code file SimpleKalmanFilter.c/h
Functions:
/*
Scalar Kalman Filter
Input
float input – input measured signal
scalar_kalman_filter_data_s* kalman_data – Kalman filter data
Return
float x estimate value
*/
float ScalarKalmanFilter(float input, scalar_kalman_filter_data_s* kalman_data)
/* Only Test */
int main() Test support
9. Download the SimpleKalmanFilterSupport.m and SimpleKalmanFilter.c/h
You can download the files:
-
SimpleKalmanFilterSupport.m
-
SimpleKalmanFilter.c/h
with the button:
10. Literature / References
[1] Colin Cowan, Peter Grant, Peter Adams “Adaptive Filters”, Prentice-Hall, ISBN 0130040371, 1985
[2] Larry J. Levy “The Kalman Filter: Navigation’s Integration Workhorse”, https://www.cs.unc.edu/~welch/kalman/Levy1997/index.html, 1997
[3] Mohinder S. Grewal, Angus P. Andrews “Kalman Filtering: Theory and Practice Using Matlab“, A Wiley-Interscience Publication, John Wiley&Sons Inc., ISBN 0-471-26638-8, 2001
Note
I recommend that you look at [1], [2] and [3].
[1] – in these papers you can read a proof of the formulas of the scalar Kalman filter
[2] – example of a scalar Kalman filter
[3] – many practical examples of Kalman filters