Simulation of the discrete linear Kalman filter

Question

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
    global Ad;
    // 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;
        Ad = zeros(4, 4);
        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, 2) = 0.0;
    Ad(1, 3) = (RRo / (LLo * (LLo + LMo))) * T;
    Ad(1, 4) = pp * wm / LLo * T;
    Ad(2, 1) = 0.0;
    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, 2) = 0.0;
    Ad(3, 3) = 1.0 - RRo / (LLo + LMo) * T;
    Ad(3, 4) = -pp * wm * T;
    Ad(4, 1) = 0.0;
    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
    P_prediction = Ad * P * Ad' + Q;
    // 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}}}$

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

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

4. $\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.

Answer 1

I have found that there was a problem with the persistent variables of the matrix type in the script above. As soon as I started to use the matrices appropriately I received more reasonable outcomes for the situation when I have artificially created a discrepancy between the model used by the Kalman filter and the model of the actual system.

1. $i_{s_{\alpha}}$, $i_{s_{\alpha_e}}$

2. $i_{s_{\beta}}$, $i_{s_{\beta_e}}$

3. $\psi_{r_{\alpha}}$, $\psi_{r_{\alpha_e}}$

4. $\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}$.