Speed sensorless robust control of PMSM

In this post I will present the speed sensorless control of PMSM. Before You start reading this article please read the following posts, because these articles are related to this topic and I don't detailing here again:

1. Extended Kalman Filter
2. Linear Quadratic Regulator
3. Dynamic model of a PMSM 

The model of the PMSM is nonlinear, to linearise the system I used the state feedback linearisation. The states can be measured or estimated. In this blog I will present a sensorless solution using the extended Kalman filter.

State estimation

For the motor model linearisation I need to know the direct axis, quadratic axis current and the speed of the rotor. 

It has the following system:

$$x_{k+1}=Ax_k+Bu_k+Ew\\y=Cx_k+Du_k\tag{1}$$

Where the states are:

$$\left[\begin{array}{c} i_d \\ i_q  \\ \omega_r  \\ \theta \\ T_L \end{array}\right]\tag{2}$$

Using the $3-8$ equations from the Dynamic model of a PMSM post, I can write the state space model of the PMSM, also I assume that the changes of the mechanical torque is small or it is constant.

$$A = \begin{bmatrix}  -\frac{R}{L_d} & \frac{L_qP\omega_r}{L_d} & 0 & 0 & 0\\ -\frac{L_dP\omega_r}{L_q} & -\frac{R}{L_q} & -\frac{PL_m}{L_q} & 0 & 0 \\ 0 & \frac{3}{2}\frac{P}{J}\left(L_m+\left(L_d-L_q\right)i_d\right) & -\frac{B}{J} & 0 & -\frac{1}{J}\\ 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 0\end{bmatrix}\tag{3}$$

$$B= \begin{bmatrix}\frac{1}{L_d} & 0 \\ 0 & \frac{1}{L_q} \\ 0 & 0\\ 0 & 0\\ 0 & 0\end{bmatrix}\tag{4}$$

The model can be discretized by using the following approach:

$$A_d=e^{AT} \approx I+AT\tag{5} $$
$$B_d=\int_0^T e^{At}B dt \approx BT\tag{6}$$
The discretized discrete state space system matrices is as follows:

$$A_k = \begin{bmatrix}  1-T_s\frac{R}{L_d} & T_s\frac{L_qP\omega_r}{L_d} & 0 & 0 & 0\\ -T_s\frac{L_dP\omega_r}{L_q} & 1-T_s\frac{R}{L_q} & -T_s\frac{PL_m}{L_q} & 0 & 0 \\ 0 & T_s\frac{3}{2}\frac{P}{J}\left(L_m+\left(L_d-L_q\right)i_d\right) & 1-T_s\frac{B}{J} & 0 & -T_s\frac{1}{J}\\ 0 & 0 & T_s & 1 & 0\\0 & 0 & 0 & 0 & 1\end{bmatrix}\tag{7}$$

$$B_k= \begin{bmatrix}\frac{T_s}{L_d} & 0 \\ 0 & \frac{T_s}{L_q} \\ 0 & 0\\ 0 & 0\\ 0 & 0\end{bmatrix}\tag{8}$$

and the Jacobian matrix is:

$$G_k=\begin{bmatrix}  1-T_s\frac{R}{L_d} & T_s\frac{L_qP\omega_r}{L_d} & T_s\frac{L_qPi_q}{L_d} & 0 & 0\\ -T_s\frac{L_dP\omega_r}{L_q} & 1-T_s\frac{R}{L_q} & -T_s\frac{PL_m+L_dPi_d}{L_q} & 0 & 0 \\ T_s\frac{3}{2}\frac{P}{2J}\left(L_d-L_q\right)i_d & T_s\frac{3}{2}\frac{P}{J}\left(L_m+\left(L_d-L_q\right)i_d\right) & 1-T_s\frac{B}{J} & 0 & -T_s\frac{1}{J}\\ 0 & 0 & T_s & 1 & 0\\0 & 0 & 0 & 0 & 1\end{bmatrix}\tag{9}$$

Filter tunning

I used an AC6 Vector Control for an interior Permanent Magnet Synchronous Motor (PMSM) simulink model to simulate and tune the estimators R,Q covariance matrices:

Figure 1.

Choosing the following covariance matrices:
$$Q=diag\left(\begin{array}{c} 1e-3 & 1e-3 & 1e-1 & 1e-9 & 1e-1\end{array}\right)\\R=diag\left(\begin{array}{c}1e3 & 1e3\end{array}\right)\tag{10}$$
the estimated speed is hence:

Rotor speed

Linear Quadratic Regulator

I already wrote a short introduction about the LQR, now I will present only the results. As I mentioned before, to control the system I need to know only the currents and the rotor velocity, so I have the following states:
$$\left[\begin{array}{c} i_d & i_q & w_r\end{array}\right]\tag{11}$$
In this case the state space model is as follows:

$$A_k = \begin{bmatrix}  1-T_s\frac{R}{L_d} & T_s\frac{L_qP\omega_r}{L_d} & 0 \\ -T_s\frac{L_dP\omega_r}{L_q} & 1-T_s\frac{R}{L_q} & -T_s\frac{PL_m}{L_q} \\ 0 & T_s\frac{3}{2}\frac{P}{J}\left(L_m+\left(L_d-L_q\right)i_d\right) & 1-T_s\frac{B}{J}\end{bmatrix}\tag{12}$$

$$B_k= \begin{bmatrix}\frac{T_s}{L_d} & 0 \\ 0 & \frac{T_s}{L_q} \\ 0 & 0\end{bmatrix}\tag{13}$$
$$C=\left[\begin{array}{c} 0 & 0 & 1\end{array}\right]\tag{14}$$
$$D=[0]$$

choosing the LQR state and input weighing matrices in this way:
$$R=\left[\begin{array}{c} 1e-8 & 1e-8 \end{array}\right]\\Q=\left[\begin{array}{c} 1e1 \end{array}\right]\tag{15}$$
and the horizon to 2, the simulation result is the following:

Results

Error between the reference speed and the simulated speed using the LQR controller

LQR rotor speed error(N=2)

The error between the reference and simulated value using the ac6_example

Rotor speed error

As you can see, in the LQR the error is between 1 and -1.5 which is better than the other controller. Still there are possibilities to reduce the error by increasing the number of the horizon and changing the weight matrices.
Here is another result:

LQR rotor speed error(N=10)

Finally, I can put together the estimator and the controller, so I will end up with the following simulink model:

EKF with LQR

And the result by choosing N=2,

The first subplot is the reference speed and the measured speed, the second is the mechanical torque and the third is the error between the reference and the measured signal.

Dynamic model of a PMSM

The two phase equivalent circuit is widely used to analyse the permanent magnet synchronous machine. In this blog I will present the two phase equivalent circuit of the PMSM dynamic model. 

The three phase electrical dynamic equations can be written as:
$$U_a^S=R_sI_a^S+\frac{d\psi_a^S}{dt}\\U_b^S=R_sI_b^S+\frac{d\psi_b^S}{dt}\\U_c^S=R_sI_c^S+\frac{d\psi_c^S}{dt}\tag{1}$$
where the index $S$ denotes the stator coordinate system.
Also the motor model can be express in the rotating coordinate system
$$U^R=R_sI^R+\frac{d\psi^R}{dt}+j\omega\psi^R\tag{2}$$

Now we can write in the two phase equivalent circuit which is rotating with the same frequency as the rotate magnetic field.

$$u_d=R_sI_d+\frac{d\psi_d}{dt}-\omega\psi_q\tag{3}$$
$$u_q=R_sI_q+\frac{d\psi_q}{dt}+\omega\psi_d\tag{4}$$
where
$$\psi_d=L_dI_d+L_m\tag{5}$$
$$\psi_q=L_qI_q\tag{6}$$
The produced torque can be expressed as:
$$T_e=\frac{3P}{2}\left(L_mI_q+(L_d-L_q)I_d\right)I_q\tag{7}$$
and the motor dynamics can be represented by:
$$T_e=J\frac{d\omega_r}{dt}+B\omega_r+T_L\tag{8}$$ 

Coordinate transformation 

Figure 1. PMSM

As you can see in figure 1, the three phase stationary reference frame can be transformed directly into a two phase reference frame using Park's transformation. Let X represent any of the variables (current, voltage, fluxe), the transformation matrix is given by[4]

$$\left[ \begin{array}{c} X_d \\ X_q \\ X_0 \end{array} \right] =\frac{2}{3} \begin{bmatrix} sin(\theta) & sin(\theta-\frac{2\pi}{3}) & sin(\theta+\frac{2\pi}{3}) \\ cos(\theta) & cos(\theta-\frac{2\pi}{3}) & cos(\theta+\frac{2\pi}{3}) \\ \frac{1}{2} & \frac{1}{2} & \frac{1}{2} \end{bmatrix}\left[ \begin{array}{c} X_a \\ X_b \\ X_c \end{array} \right]\tag{9}$$

The transformation can be a combination of two transformations. First the three phase reference frame can be transformed into a two phase reference frame($abc$ to $\alpha\beta$) by replacing $\theta$ with 0

$$\left[ \begin{array}{c} X_\alpha \\ X_\beta \\ X_0 \end{array} \right] =\frac{2}{3} \begin{bmatrix} 0 & -{\sqrt3\over2} & {\sqrt3\over2} \\ 1 & -{1\over2} & -{1\over2} \\ \frac{1}{2} & \frac{1}{2} & \frac{1}{2} \end{bmatrix}\left[ \begin{array}{c} X_a \\ X_b \\ X_c \end{array} \right]\tag{10}$$

The second transformation is a conversion from stationary to rotating reference frame ($\alpha\beta$ to $dq$)

$$\left[ \begin{array}{c} X_d \\ X_q \\ X_0 \end{array} \right] = \begin{bmatrix}  cos(\theta) & sin(\theta) & 0 \\ -sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1 \end{bmatrix}\left[ \begin{array}{c} X_\alpha \\ X_\beta \\ X_0 \end{array} \right]\tag{9}$$

Appendix:

$P$ - Pole pairs
$R$ - Stator phase resistance
$L_m$ - Permanent magnets flux

$B$ - Viscous friction coefficient
$J$ - Inertia
$L_d$ - Direct axis inductance
$L_q$ - Quadrature axis inductance

$\omega$ - Angular velocity

Sources:

1. Dal Y. Ohm: DYNAMIC MODEL OF PM SYNCHRONOUS MOTORS
2. Wikipedia: dqo transformation
3. Mohamed S. Zaky: Adaptive and robust speed control of interior permanent magnet synchronous motor drives
4. Lecture Set 6.pdf

LQ speed regulation of a brushed DC motor

In the following post I will present a speed controller of the brushed DC motor using LQR. I will also introduce the Kalman filter to estimate the motor speed and torque. 

System diagram

I already introduced the state space model of the brushed DC machine, in this post I will introduce only the controller. 

Discrete state tracking linear quadratic regulator

Have the following linear discrete system

$$x_{k+1}=A_kx_k+B_ku_k\\
y_k=H_kx_k\tag{1}$$

Let us presume that $z_k$ is the trajectory what we would like to follow, in this case we have to determine the optimal control signal which optimize the quadratic criterion:
$$
J(u) = min\left[e_N^TFe_N+\sum_k{e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k}\right]\tag{2}
$$
Where  $e=z_k-y_k$ and $\delta u_k=u_k-up_k$.
Bellman's equation of the corresponding control problem is:
$$
J_{min_k}(u) = min\left[e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k+J_{min_{k+1}}(u)\right]\tag{3}
$$
Expressing the cost of termination point as:
$$
J_{min_N}(u)=\left[z_N-H_Nx_N\right]^TF\left[z_N-H_Nx_N\right]\tag{4}
$$Introduce the following expressions:
$$P_N=H_N^TF_NH_N\\ p_{N_0}=z_N^TFz_N\\ g_N^T=-2z_N^TFH_N\tag{5}$$
Where $N$ is the number of the horizon and $k$ is the iteration.
The cost function will be the following:
$$
J_{min_k}(u)=p_{k_0}+g_k^Tx_k+x_k^TP_kx_k\tag{6}
$$
where the Belman's equation is:
$$
J_{min_k}(u)=min\left[e_k^TQ_ke_k+\delta u_k^TR_k\delta u_k+p_{(k+1)_0}+g_{k+1}^Tx_{k+1}+x_{k+1}^TP_kx_{k+1}\right]\tag{7}
$$
Considering that in the $k$ iteration the $u_k$ optimal control minimizes the criterion $J(u)_k$, we can write the following relation
$$u_k=\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\left[R_kup_k+\frac{1}{2}B_kg_{k+1}-B_k^TP_{k+1}A_kx_k\right]\tag{8} $$
The recursive discrete Ricatti equation is the following:$$P_k=H_k^TQ_kH_k-A_k^TP_{k+1}B_k\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\\B_k^TP_{k+1}A_k+A_k^TP_{k+1}A_k\tag{9}$$respecting the final condition:
$$P_N=H_N^TF_NH_N\tag{10}$$
We can express the optimal control signal in the following form:
$$u_k=K_kx_k+f_k\tag{11}$$
where
$$K_k=-\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}B_k^TP_{k+1}A_k\tag{12}$$
$$f_k=-\frac{1}{2}\left[R_k+B_k^TP_{k+1}B_k\right]^{-1}\left[B_k^Tg_{k+1}\right]\tag{13}$$
After performing some algebraic calculations, we will end up with the following equation:$$g_k^T=-2z_k^TQ_kH_k+g_{k+1}\left[A_k+B_kK_k\right]^T\tag{14}$$
with boundary condition:
$$g_N^T=-2z_N^TFH_k\tag{15}$$

Results 

The steady state error depends on the number of the horizon ($N$), the accuracy of the system model and the state estimation. Choosing the number of the horizon to five the rotor speed is the following:

 The controller signal is depending of the accuracy of the state estimator

Choosing the number of the horizon more than 15, the steady state error was 0. The number of the horizon, and the limitation of the control signal are influences the overshoot. One of the disadvantages of the LQR is that the optimal signal calculation doesn't include the signal limitation, thus a bigger control signal can be calculated than you can physically achieve.

Python implementation

The Python implementation of the state tracking LQR is under the following link. Feel free to fork. It was harder to implement into Python than Matlab, but finally I succeeded. In the T_DCMotor.py you can find two controller, one is a PID controller

pid  =  PID( KP = 0.2, KI = 100, KD = 0.00001 
for i in range( len( pw ) ):
    #Calculate the error
    error = pw[i] - sw;
    #Calculating the control signal
    u = pid.run( error, Ts = Ts )
    if( abs( u ) > 220 ):
        u = sign( u ) * 220; 

    #Simulating
    ( sw, x ) = dc2.dlsim( u , torque[0, i ] );
    y_out[i, :] = sw
    u_out[i, :] = u 

The other is the LQR state tracker controller with Kalman filter

dlqr = DSTLQR( Ak, Bk, Ck, N )
kf = dKF( x_est, Ak, Bk, H )

for i in range( len( pw ) - N ): 
   #Calculation the optimal control signal
   U = dlqr.calcU( x_est, array( [pw[i:i + N]] ) ) 
   if( abs( U ) > 220 ):
     U = sign( U ) * 220;

   #Simulating
   ( sw, x ) = dc2.dlsim( U, torque[0, i ], Ts = Ts );
   y_out[i, :] = sw
   u_out[i, :] = U

   #Estimating the states
   ( x_est, P, error ) = kf.update( x[0, 0] , U )

   y_est[i, :] = x_est[1]; 

Matlab source code

I used a discrete Kalman filter to estimate the torque and the rotor speed, here is the code of the Kalman filter:

% Created by Róbert Ungurán
% GPLv2
%******************************************************
% Dc motor discrete model
%******************************************************
% Input 
%   I_in - Measured current
%   U_in - Measured voltage
%******************************************************
% Output
%   x_out - Estimated state variables
function x_out = kf(I_in,U_in)

    global Ts  Rm L Ke Kd J Km;
    
    persistent x
    persistent P
    if isempty(x)
        x = [0;0; 0];        
        P = zeros(3);
    end
    % Armature current, rotor speed, Torque
    Ak  =   [1-Ts*(Rm/L)  -Ts*(Ke/L)        0;
            Ts*(Km/J)    1-Ts*(Kd/J)    -Ts/J;
            0               0               1;];

    Bk  =   [Ts*1/L; 0; 0];

    H   =   [1 0 0];
    
    
    R   =   1e-3;
    Q   =   diag([1e-6 1e-2 1e-2]);
    
    
    x   =   Ak*x + Bk*U_in;
   
    P   =   Ak * P * Ak' + Q;
    
   
    % Compute Kalman gain factor:
    S   =   H*P*H'+R;
    K   =   P*H'*inv(S);

    % Correction based on observation:
    error = I_in-H*x;
    x   =   x + K*error;
    P   =   P - K*H*P;
    
    x_out   =   x;
end

The source of the discrete linear quadratic regulator

% Created by Róbert Ungurán
% GPLv2
%******************************************************
% LQR controller of a DC motor
%******************************************************
% Input 
%   I       - Measured current
%   w       - Measured rotor speed
%   w_ref   - Reference rotor speed 
%******************************************************
% Output
%   U - Control signal
function U = mydlqr(I,w,Tl,w_ref)   
  
    global Ts  Rm L Ke Kd J Km N;
    
    x = [I;w;Tl];

    Ak  =   [1-Ts*(Rm/L)  -Ts*(Ke/L)        0;
            Ts*(Km/J)    1-Ts*(Kd/J)    -Ts/J;
            0               0               1;];

    Bk = [Ts*1/L; 0; 0];

    Ck = [0 1 0];

    Dk = [0];   
   
    
    [n,m] = size(Bk);
    [p,n] = size(Ck);
    
    R=eye(m)*0.001;
    Q=eye(p)*10;
    F=Q;
    
    PN = Ck'*F*Ck; 
    gN =  (-2*w_ref(1)'*F*Ck)';    
    
    
    for j=N:-1:1        
        K = (-inv(R+Bk'*PN*Bk)*Bk'*PN*Ak);
        fk = (-1/2)*inv(R+Bk'*PN*Bk)*Bk'*gN;       
        gN = (-2*w_ref(:,N-1)'*Q*Ck+gN'*(Ak+Bk*K))';
        PN = Ck'*Q*Ck-Ak'*PN*Bk*inv(R+Bk'*PN*Bk)*Bk'*PN*Ak+Ak'*PN*Ak;            
    end
     
    U = K*x+fk;
    
end

The main source code

clear all
close all
clc

%% Initialization
global Ts  Rm L Ke Kd J Km N Fc;

Rm  =   0.35;
Km  =   0.0296;
Ke  =   0.0296;
Kd  =   6.7*10e-5;
J   =   2.9*10e-6;
Fc  =   0.02;
L   =   25 *10e-6;
Ts  =   1e-5;
N   =   5;

x       =   [0;0;0];
w_ref   =   [ones(1,1000)*200 ones(1,1000)*400];
torque  =   [ones(1,1500)*2.0 ones(1,500)*1.0];
[n,m]   =   size(w_ref);
u   =   [];
w   =   [];
x_est = [0; 0; 0];
x_est_o = [0; 0; 0];


for i=1:m-N
    %% Calculation the optimal control signal
    %Input Current, Rotation speed, Rotation speed references
    U   =   mydlqr(x_est(1),x_est(2),x_est(3), w_ref(i:i+N));
    if(abs(U) > 380)
        U = sign(U)*380;
    end
    u   =   [u U];
    
    %% Simulation the model
    %Current, Rotation speed, Rotation position    
    [x(1), x(2), x(3)]  =   dc_motor(U, torque(i));
    w   =   [w x(2)];
    %% Estimating the states
    x_est   =   kf(x(1), U);
    
    x_est_o =   [x_est_o x_est];    
    
end

%% Visualization
plot(u);
figure;
plot(x_est_o(3,:));
figure;
hold on;
plot(w_ref, 'b-');
plot(w, 'r-');
plot(x_est_o(2,:), 'g-');
legend('Reference','Simulated', 'Estimated');

ACIM speed estimation

In case if we would like to know the induction motor rotor speed, we can use Extended Kalman Filter(EKF) to estimate the unknown states. Here is a good tutorial where you can find more information about the EKF. Also you can read the article on the Wikipedia. I'm using the FOC block in Matlab to get the motor control current, voltage and the angle between the fluxes, just for testing purpose.
I assume that the rotor speed is constant so $\frac{d\omega_r}{dt}=0$. In this case we have the following model

$$
A= \begin{bmatrix} -\frac{R_s+\frac{L_m^2R_r}{L_r^2}}{L_s\sigma} & 0 &  \frac{L_m}{L_s\sigma L_r\tau_r} & \frac{\omega_rL_m}{L_s\sigma L_r} & 0 \\ 0 & -\frac{R_s+\frac{L_m^2R_r}{L_r^2}}{L_s\sigma}  & -\frac{\omega_rL_m}{L_s\sigma L_r} & \frac{L_m}{L_s\sigma L_r\tau_r} & 0 \\ \frac{L_m}{\tau_r} & 0 & -\frac{1}{\tau_r} & -\omega_r & 0 \\ 0 & \frac{L_m}{\tau_r} &   \omega_r & -\frac{1}{\tau_r}  & 0 \\ 0 & 0 & 0 & 0 & 0\end{bmatrix}\tag{1}
$$

$$B=\begin{bmatrix} \frac{1}{L_s\sigma} & 0 \\ 0 & \frac{1}{L_s\sigma} \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \end{bmatrix}\tag{2}$$

We can discretize the model using the following approach:

$$A_d=e^{AT} \approx I+AT\tag{3} $$
$$B_d=\int_0^T e^{At}B dt \approx BT\tag{4}$$

The Simulink model is the following

In the block above, the second and third module are the Clarke's and Park's transformation, in the previous articles I already defined this transformations. The code of the speed and flux estimator is

function [xx_out,PP_out,error,W]=ekf(I,U,Ts)
% EKF   Extended Kalman Filter  
% induction motor's parameters   
persistent PP;
persistent xx;

if isempty(PP)
    xx = [0 0 0 0 0 ]';
    %[i_alfa , i_beta , Fi_r_alfa , Fi_r_beta , W_r ]
    PP = 1e-2*diag([1 1 1 1 1]);
end
Q =  diag([1e-4 1e-4 1e-9 1e-9 5e-4]);
R =  1e1*diag([1 1]);

L_r=0.479;
L_s=0.423;
L_m=0.421;
R_s=5.27;
R_r=5.07;

tau_r = L_r/R_r;
gamma_r = 1-(L_m^2)/(L_s*L_r);
T_r = R_s+(L_m^2*R_r)/L_r^2;
 
Ak =[ 1-Ts*(T_r/(L_s*gamma_r)),          0,        Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),        Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),        0;
    0,                  1-Ts*(T_r/(L_s*gamma_r)), -Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),     Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),        0;
    Ts*(L_m/tau_r),                  0,                   1-Ts*(1/tau_r),                  -Ts*xx(5),                                    0;
    0,                      Ts*(L_m/tau_r),                  Ts*xx(5),                      1-Ts*(1/tau_r),                         0;
    0                           0                           0                    0                                           1];

Bk = [Ts/(L_s*gamma_r)              0;
            0                Ts/(L_s*gamma_r);
            0                       0;
            0                       0;
            0                       0];

%Predictor
xx = Ak*xx+Bk*U;


Gk =[ 1-Ts*(T_r/(L_s*gamma_r)),          0,        Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),        Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),        Ts*((xx(4)*L_m)/(L_s*gamma_r*L_r));
    0,                  1-Ts*(T_r/(L_s*gamma_r)), -Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),     Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),        -Ts*((xx(3)*L_m)/(L_s*gamma_r*L_r));
    Ts*(L_m/tau_r),                  0,                   1-Ts*(1/tau_r),                  -Ts*xx(5),                                    -Ts*xx(4);
    0,                      Ts*(L_m/tau_r),                  Ts*xx(5),                      1-Ts*(1/tau_r),                         Ts*xx(3);
    0                           0                           0                           0                                           1];

H = [1 0 0 0 0;
     0 1 0 0 0;];

PP=Gk*PP*Gk'+Q;

%Coorection
KalG=PP*H'*inv(H*PP*H'+R);
error = I-H*xx;
xx=xx+KalG*(error);
PP=(eye(5)-KalG*H)*PP;

xx_out = xx;
PP_out=PP;

W = xx(5);

I spent a lot of time to tune the filter matrices Q,R, but this is the best result what I got.
I wasn't satisfied with the result above so I extended this model with the torque, in this case the rotor speed can be expressed as follows:
$$
\frac{d\omega_r}{dt} =\frac{3p^2L_m}{8JL_r}\left(i_{sq}\psi_{rd}-i_{sd}\psi_{rq}\right)-\frac{p}{2J}T_l\tag{5}
$$

we assume that the torque is constant or the changes between two sampling time is very small

$$
\frac{dT_l }{dt}=0\tag{6}
$$
In this case the EKF's code is

function [xx_out,PP_out,error,W]=ekf(I,U,Ts)
% EKF   Extended Kalman Filter  
% induction motor's parameters   
persistent PP;
persistent xx;

if isempty(PP)
    xx = [0 0 0 0 0 0]';
    %[i_alfa , i_beta , Fi_r_alfa , Fi_r_beta , W_r ]
    PP = 1e-8*diag([1 1 1 1 1 1]);
end
Q =  diag([1e-3 1e-3 1e-9 1e-9 1e-5 1e-5]);
R =  1e-3*diag([1 1]);

L_r=0.479;
L_s=0.423;
L_m=0.421;
R_s=5.27;
R_r=5.07;
p = 2;
J =0.02;
fv = 0.0038;

m = (3*p^2*L_m)/(8*J*L_r);

tau_r = L_r/R_r;
gamma_r = 1-(L_m^2)/(L_s*L_r);
T_r = R_s+(L_m^2*R_r)/L_r^2;
 
Ak =[ 1-Ts*(T_r/(L_s*gamma_r)),          0,        Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),        Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),             0       0;
    0,                  1-Ts*(T_r/(L_s*gamma_r)), -Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),     Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),                0       0;
    Ts*(L_m/tau_r),                  0,                   1-Ts*(1/tau_r),                  -Ts*xx(5),                                       0       0;
    0,                      Ts*(L_m/tau_r),                  Ts*xx(5),                      1-Ts*(1/tau_r),                                 0       0;
    -Ts*m*xx(4)                  Ts*m*xx(3)                       0                               0                                   1           (-p*Ts)/(J*2);
           0                    0                               0                               0                                   0               1];

Bk = [Ts/(L_s*gamma_r)              0;
            0                Ts/(L_s*gamma_r);
            0                       0;
            0                       0;
            0                       0;
            0                       0];

%Predictor
xx = Ak*xx+Bk*U;


Gk =[ 1-Ts*(T_r/(L_s*gamma_r)),          0,        Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),        Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),        Ts*((xx(4)*L_m)/(L_s*gamma_r*L_r))   0;
    0,                  1-Ts*(T_r/(L_s*gamma_r)), -Ts*((xx(5)*L_m)/(L_s*gamma_r*L_r)),     Ts*(L_m/(L_s*gamma_r*L_r*tau_r)),        -Ts*((xx(3)*L_m)/(L_s*gamma_r*L_r))     0;
    Ts*(L_m/tau_r),                  0,                   1-Ts*(1/tau_r),                  -Ts*xx(5),                                    -Ts*xx(4)                          0;
    0,                      Ts*(L_m/tau_r),                  Ts*xx(5),                      1-Ts*(1/tau_r),                                 Ts*xx(3)                        0;
    -Ts*m*xx(4)              Ts*m*xx(3)                       Ts*m*xx(2)                       -Ts*m*xx(1)                                      1                           (-p*Ts)/(J*2);
    0                               0                           0                           0                           0                                           1];                         
             
H = [1 0 0 0 0 0;
     0 1 0 0 0 0;];

PP = Gk*PP*Gk'+Q;


KalG = PP*H'*inv(H*PP*H'+R);
error = I-H*xx;
xx = xx + KalG*(error);
PP = (eye(6)-KalG*H)*PP;

xx_out = xx;
PP_out = PP;

W = xx(5);

The result is

The error between the measured and the estimated is smaller than the previous model, also it was easier to tune the estimator. The disadvantage of the model, that is has six state which means that the calculations takes longer than the previous model.