Model predictive control of brushed DC motor

Motor model

The state space model of a brushed DC motor as follow:
$$A =\left[ {\begin{array}{cc} -\frac{R_{a}}{L_{a}} & -\frac{K_{e}}{L_{a}} &  0 \\ \frac{K_{m}}{J} & -\frac{b}{J} & 0 \\ 0 & 1 & 0 \end{array} } \right] B=\left[ {\begin{array}{cc} \frac{1}{L_{a}} \\ 0 \\ 0 \end{array} } \right] C=\left[ {\begin{array}{cc} 0 \\ 1 \\ 0 \end{array} } \right] D=\left[ {\begin{array}{cc} 0 \end{array} } \right]\tag{1}$$
As you can see, I assumed that the torque is zero. 
Where the states are:
$$\dot{x(t)} =\left[ {\begin{array}{cc} \frac{di}{dt} \\ \frac{d\omega}{dt} \\ \frac{d\theta}{dt} \end{array} } \right]\tag{2}$$

Controller design

I already introduced the MPC controller, please read that first before you continue to read this blog. 

The first step is to determine the Toeplitz matrix:

$$\begin{bmatrix}x_{k+1|k} \\ \vdots \\ x_{k+N_c|k} \\ x_{k+N_c+1|k} \\ \vdots \\ x_{k+N|k} \end{bmatrix}=\begin{bmatrix}\Phi \\ \vdots \\ \Phi^{N_c} \\ \Phi^{N_c+1} \\ \vdots \\ \Phi^N \end{bmatrix}x_k + \begin{bmatrix}\Gamma \\ \vdots \\ \sum_{i=0}^{N_c-1} \Phi^{i} \Gamma\\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma\\ \vdots \\ \sum_{i=0}^{N}\Phi^i \Gamma \end{bmatrix}u_{k-1}\\+ \begin{bmatrix}\Gamma & \cdots & 0 \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N_c}\Phi^{i} \Gamma & \cdots & \Phi \Gamma+\Gamma \\ \vdots & \ddots & \vdots \\ \sum_{i=0}^{N-1}\Phi^i \Gamma & \cdots & \sum_{i=0}^{N-N_c}\Phi^i \Gamma \end{bmatrix} \begin{bmatrix}\Delta u_{k+1|k} \\ \vdots \\ \Delta u_{k+N_c-1|k} \end{bmatrix}\tag{1}$$ 

I wrote the following Matlab function to build up the  $\Phi^*$, $\Gamma^*$, $G_y$:

function [FiFi,FiGam,Gy] = getFiFi_FiGa(Fi,Ga,C,N,Nc,n)
    
    FiFi=[]; %ennek a merete N*n x n kell legyen
    
    for i=1:N,    
        FiFi=[FiFi; C*Fi^i]; 
    end
    % FiGam szamitasa
    FiGam=[];
    for i=1:N,
        S=0;
        for j=0:i-1,       
            S=S+C*Fi^j*Ga;    
        end
        FiGam=[FiGam; S];
    end
    
    ng=size(FiGam,1);
    mg=size(FiGam,2);
    % Gy szamitasa
    Gy=zeros(ng,mg*Nc);
    % Gy szamitasa
    for j=1:mg:Nc*mg   % oszlopok         
        Gy(j:end, j:j+mg-1)=FiGam(1:end-j+1,:);
    end

end

To find the optimal control signal we have to solve the following optimization problem $J_k(u,x_k)=\frac{1}{2}\Delta u_k^TH\Delta u_k+f^T\Delta u_k+const$. I already presented the solution of this problem (see Model predictive control). I am going to  implement the calculation of the $\Delta u_k$ optimal control signal in the following example:

for v=1:n-N
    %Error vector
    E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
    
     deltau=inv(Gy'*Q*Gy+R)*Gy'*Q*E;
     u=u+deltau(1:2);
     for k=1:length(u)
         if(abs(u(k))>lim)
             u(k) = sign(u(k))*lim;
         end
     end
   
    x0 = Fi*x0+Ga*u;
    yt = C*x0;
    
    
    um=[um u];
    ym=[ym;yt];
end;

In the example above I also tried to constrain the control signal by choosing the $lim$ variable to 38. After choosing the values of the noise matrices as follows

• $Q=eye(N*Nro)*1e4$ where $N$ is the number of the horizon and $Nro$ is the number of the outputs.
• $R=eye(Nc*Nrin)*1e-5$ where $Nc$ is the number of the control horizon and $Nrin$ is the number of the inputs.

I got these results:

Figure 1. Rotor speed

Figure 2. Control signal

Figure 3. Error between the reference and measured rotor speed

We can reduce the error by increasing or eliminating the constrain of control signal as you can see in $Figure 4$

Figure 4. Error between the reference and measured rotor speed without limiting the control signal

The other solution  of the optimal control signal calculation in Matlab is the built in $quadprog$ function. You can find a simple example below, also you can read the description of this function on the Matlab help page:

H=2*(Gy'*Q*Gy+R);
for v=1:n-N
    %Hiba vektor    
    E=Yref(v:v+N-1)-FiFi*x0-FiGam*u;
    
    f=-2*E'*Q*Gy;        
    
    b = [abs(lim-u(1));
        abs(lim+u(1))];
    A = [1 0 0 0 0 0 0 0;
        -1 0 0 0 0 0 0 0;
        ];
     
    Aeq=[];beq=[];
    
    [deltau,FVAL,EXITFLAG]=quadprog(H,f,A,b,Aeq,beq);
    u=u+deltau(1:2);
    
   
    x0 = Fi*x0+Ga*u;
    yt = C*x0;    
    
    um=[um u0];
    ym=[ym;yt];
    Em = [Em;E];
end;

Appendix

Motor parameters:

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