Interpolation and approximate feedback for an linear quadratic regulator (LQR) problem

72 Views Asked by At

The problem is to perform the following numerical experiment:

Suppose i have a second order controlled dynamical system: $$\dot{x}_1(t)=x_2(t)~,~x_1(0)=2\\\dot{x}_2(t)=-2x_1(t)+x_2(t)+u(t)~,~x_2(0)=-3$$ And the performance index is $$J:=\int_0^{\infty}2x_1^2+6x_1x_2+5x_2^2+0.25u^2 \mathrm{d}t$$ The system matrices are: $A=\begin{pmatrix}0 & 1 \\-2 & 1 \\ \end{pmatrix}, B=\begin{pmatrix}0 \\1\\ \end{pmatrix}, Q=\begin{pmatrix}2 & 3 \\3 & 5 \\ \end{pmatrix}, R=\frac{1}{4}$. Next i have solved the system in MATLAB and found the state and control trajectories as follows:

clear all
close all
clc
x10=2; 
x20=-3; 
X0=[x10;x20];
A=[0 1;-2 1]; 
B=[0;1]; 
Q=[2 3;3 5]; 
R=[0.25]; 
[K,P,EV]=lqr(A,B,Q,R) 
BIN=[0;0]; 
C= [1 1];
D= [1] ;
tfinal=10;
t=0:0.05:tfinal;
[Y,X,t]=initial(A-B*K,BIN,C,D,X0,t);
x1t=[1 0]*X'; 
x2t=[0 1]*X';
ut=-K*X' ;
figure(1)
plot(t,x1t, 'k' ,t,x2t, 'k'); grid on
xlabel ( 't' )
figure(2)
plot (t ,ut , ' k ' ); grid on
xlabel ('t')

My outputs are : States State plots Control

enter image description here

Now the question says :"make-believe" that you know the values of feedback only on a certain grid, construct such a grid by specifying 'h', and evaluate optimal feedback at those points only, and use some quasi interpolation to build approximate feedback for entire state space $\mathbb{R}^2$.

My queries are as follows:

(1) so here the gain $K \in \mathbb{R}^{1 \times 2}$, as far as i understand i need to grid the states $x_1(t)$ and $x_2(t)$ by some step size 'h' on a certain interval, and then evaluate the feedback $\left[k_1x_1~k_2x_2\right]$ and then use those values to interpolate, am i right?

(2) So after solving using the above code i got the states $x_1$ and $x_2$ as some $(201 \times 1)$ vector for the time steps i have chosen $t=0$ to $t_f=10$ with step $0.05$, now i don't understand how to grid the $x_1(t)$ and $x_2(t)$ vector using some grid size on some region ? if i'm able to do that i can find the optimal feedback by multiplying K component-wise with the gridded states and then interpolate.

i'm not sure if i'm thinking in the right direction, any help would be appreciated.