2
$\begingroup$

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}}}$

enter image description here

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

enter image description here

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

enter image description here

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

enter image description here

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.

$\endgroup$
6
  • $\begingroup$ 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. $\endgroup$
    – Peter K.
    Commented Dec 13, 2022 at 13:40
  • $\begingroup$ @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. $\endgroup$
    – Steve
    Commented Dec 13, 2022 at 14:17
  • $\begingroup$ 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); $\endgroup$
    – Steve
    Commented Dec 13, 2022 at 14:27
  • $\begingroup$ 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);. $\endgroup$
    – Peter K.
    Commented Dec 13, 2022 at 14:32
  • $\begingroup$ 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. $\endgroup$
    – TimWescott
    Commented Dec 14, 2022 at 15:42

1 Answer 1

1
$\begingroup$

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}}$

enter image description here

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

enter image description here

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

enter image description here

  1. $\psi_{r_{\beta}}$, $\psi_{r_{\beta_e}}$

enter image description here

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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.