# Simulation of the discrete linear Kalman filter

I have been working on a Scilab simulation of the discrete Kalman filter which is used as a state observer of the linear dynamic system. The Scilab script for the discrete Kalman filter is as follows

function [is_alpha_estimate, is_beta_estimate, psir_alpha_estimate, psir_beta_estimate] = MachineModel(vs_alpha, vs_beta, is_alpha, is_beta, wm)

global init;

// system matrix
// input matrix
global Bd;
// output matrix
global Cd;

// system input in last sampling instant
global u_previous;
// system state estimate prediction
global x_estimate_prediction;
// system state estimate
global x_estimate;
// system output
global y;
// covariance matrix of the process noise - the dispersions of the process variables on the main diagonal
global Q;
// covariance matrix of the measurement noise - the dispersions of the measurements on the main diagonal
global R;
// covariance matrix of the estimate error
global P;
// covariance matrix of the prediction estimate error
global P_prediction;

if isempty(init) then
init = 1;
Bd = zeros(4, 2);
Cd = zeros(2, 4);
u_previous = zeros(2, 1);
x_estimate_prediction = zeros(4, 1);
x_estimate = zeros(4, 1);
y = zeros(2, 1);
Q = 5*eye(4, 4);
// I suppose accurate measurement
R = zeros(2, 2);
P = 50*eye(4, 4);
P_prediction = zeros(4, 4);
end

// system matrix
Ad(1, 1) = 1.0 - ((RSo * (LLo + LMo) * (LLo + LMo) + RRo * LMo * LMo) / (LLo * LMo * (LLo + LMo))) * T;
Ad(1, 3) = (RRo / (LLo * (LLo + LMo))) * T;
Ad(1, 4) = pp * wm / LLo * T;
Ad(2, 2) = 1.0 - ((RSo * (LLo + LMo) * (LLo + LMo) + RRo * LMo * LMo) / (LLo * LMo * (LLo + LMo))) * T;
Ad(2, 3) = -pp * wm / LLo * T;
Ad(2, 4) = (RRo / (LLo * (LLo + LMo))) * T;
Ad(3, 1) = (LMo * RRo) / (LLo + LMo) * T;
Ad(3, 3) = 1.0 - RRo / (LLo + LMo) * T;
Ad(3, 4) = -pp * wm * T;
Ad(4, 2) = (LMo * RRo) / (LLo + LMo) * T;
Ad(4, 3) = pp * wm * T;
Ad(4, 4) = 1.0 - RRo / (LLo + LMo) * T;

// input matrix
Bd(1, 1) = (LLo + LMo) / (LLo * LMo) * T;
Bd(1, 2) = 0.0;
Bd(2, 1) = 0.0;
Bd(2, 2) = (LLo + LMo) / (LLo * LMo) * T;
Bd(3, 1) = 0.0;
Bd(3, 2) = 0.0;
Bd(4, 1) = 0.0;
Bd(4, 2) = 0.0;

// output matrix
Cd(1, 1) = 1.0;
Cd(1, 2) = 0.0;
Cd(1, 3) = 0.0;
Cd(1, 4) = 0.0;
Cd(2, 1) = 0.0;
Cd(2, 2) = 1.0;
Cd(2, 3) = 0.0;
Cd(2, 4) = 0.0;

// system output
y(1, 1) = is_alpha;
y(2, 1) = is_beta;

// prediction calculation
x_estimate_prediction = Ad * x_estimate + Bd * u_previous;
u_previous(1, 1) = vs_alpha;
u_previous(2, 1) = vs_beta;

// correction gain calculation
// covariance matrix of the prediction estimate
// correction gain
K = P_prediction * Cd' * inv(Cd * P_prediction * Cd' + R);

// correction calculation
x_estimate = x_estimate_prediction + K * (y - Cd * x_estimate_prediction);
// covariance matrix of the estimate
P = P_prediction - K * Cd * P_prediction;

is_alpha_estimate = x_estimate(1, 1);
is_beta_estimate = x_estimate(2, 1);
psir_alpha_estimate = x_estimate(3, 1);
psir_beta_estimate = x_estimate(4, 1);

endfunction


My goal was to do some very first experiments with the Kalman filter and further explore the influence of the matrices $$\mathbf{Q}$$, $$\mathbf{R}$$, $$\mathbf{P}$$. During those experiments I have noticed that irrespect of the estimates of the variances in the matrices $$\mathbf{Q}$$, $$\mathbf{R}$$ and initial estimates of the variances in the matrix $$\mathbf{P}$$ I still receive the same state estimates which are given below

1. $$i_{s_{\alpha}}$$, $$i_{s_{\alpha_{e}}}$$

1. $$i_{s_{\beta}}$$, $$i_{s_{\beta_{e}}}$$

1. $$\psi_{r_{\alpha}}$$, $$\psi_{r_{\alpha_{e}}}$$

1. $$\psi_{r_{\beta}}$$, $$\psi_{r_{\beta_{e}}}$$

From the above given graphs it is apparent that the estimates of the first two state variables ($$i_{s_{\alpha}}$$, $$i_{s_{\beta}}$$) are exactly the same as the actual values (both the curves are overlaped). On the other hand the estimates of the second two state variables ($$\psi_{r_{\alpha}}$$, $$\psi_{r_{\beta}}$$) have significant error which tends to increase as time goes.

In my opinion there are two reasons for this behavior:

1. I have implemented the discrete Kalman filter in wrong manner
2. There is something wrong in my Scilab simulation

To be able to decide where I have to look for further I would like to ask you for a review of the script above whether you can see any mistake. Thanks in advance.

• The code you've included doesn't seem to do the Kalman filter. The Kalman filter would use y = Cd*x_estimate; to form these two y(1, 1) = is_alpha; and y(2, 1) = is_beta; instead of just making the output values that are passed into the function. That's probably not the problem, but it's one thing that seems awry.
– Peter K.
Dec 13, 2022 at 13:40
• @PeterK. thank you for your reaction. The idea behind those two lines was to pass the measured output of the system to the Kalman filter. The y is then used in the correction step for comparison with the estimated output which is really constructed via Cd*x_estimate. Dec 13, 2022 at 14:17
• As far as the output of the Kalman filter it is on these lines is_alpha_estimate = x_estimate(1, 1); is_beta_estimate = x_estimate(2, 1); psir_alpha_estimate = x_estimate(3, 1); psir_beta_estimate = x_estimate(4, 1); Dec 13, 2022 at 14:27
• I see! Yes, that makes sense. I can see it's used correctly in this line: x_estimate = x_estimate_prediction + K * (y - Cd * x_estimate_prediction);.
– Peter K.
Dec 13, 2022 at 14:32
• You keep saying "initial conditions of $\mathbf Q$, $\mathbf R$, and $\mathbf P$. But for a plain ol' Kalman filter, $\mathbf Q$ and $\mathbf R$ do not evolve with the filter -- they're defined by the problem, either as fixed quantities or as time-varying parameters that are already known. One might vary them if the filter is an extend or unscented Kalman filter, but I don't see you doing that in your code. Dec 14, 2022 at 15:42

1. $$i_{s_{\alpha}}$$, $$i_{s_{\alpha_e}}$$
1. $$i_{s_{\beta}}$$, $$i_{s_{\beta_e}}$$
1. $$\psi_{r_{\alpha}}$$, $$\psi_{r_{\alpha_e}}$$
1. $$\psi_{r_{\beta}}$$, $$\psi_{r_{\beta_e}}$$
Nevertheless these outcomes are still the same irrespect to the estimates of the variances in the matrices $${\mathbf Q}$$, $${\mathbf R}$$ and the initial estimate of the variance in the matrix $${\mathbf P}$$.