 ## Kalman Filter with Example. C Code and Octave Script

### 1. Abbreviation     ### 2. Introduction

##### See the Figure 2-1. ### 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: 1. ##### Prediction Step: 2. ##### Kalman Gain Calculation: 3. ##### Measurement of the 4. ##### Calculation of the output values of the Kalman filter: 1. ##### Calculation of Kalman Gain by the formula: 3. ##### Calculation of the output values of the Kalman filter: 1. ##### Prediction Step: 2. ##### Kalman Gain Calculation: 3. ##### Measurement of the 4. ##### Calculation of the output values of the Kalman filter: ### 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): ##### 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) ### 7. Octave GNU Script KalmanFilterExampleSupport.m

##### 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 estimate value
*/
float KalmanFilterExample(float input, kalman_filter_data_s* kalman_data)