【LQR】离散代数黎卡提方程的求解，附Matlab/python代码（笔记）

LQR的核心是设计QRN，并求解对应的黎卡提方程

对于连续状态空间方程系统，先求连续LQR后离散 和 先离散后求离散LQR方程 的结果 是不一样的

1.离散代数黎卡提方程

注：LQR算法中含N项

离散系统：

在matlab里有现成的函数dlqr()，但为了搞清楚其内核，编写matlab代码展示其求解过程

matlab帮助文件里的dlqr()说明
对于离散代数黎卡提方程的求解，红圈3是关键，将其中的S单独拿出，即可转化为：

S0=A'*S*A-(A'*S*B+N)*inv(B'*S*B+R)*(B'*S*A+N')+Q

其中等号左边的S0认为是S(k+1)，右边的S认为是S(k)

此公式迭代即可，采用下文的迭代思想（仅仅参考迭代法的思想）：
迭代法求解非线性方程的前提

2.matlab代码

clc
clear
close all
%% 1.参数
mb=240;
mt=30;
ks=16000;
kt=160000;
A0=[0 1 0 -1;
    -ks/mb 0 0 0;
    0 0 0 1;
    ks/mt 0 -kt/mt 0];
B0=[0;-1/mb;0;1/mt];
G0=[0;0;-1;0];
C0=[-ks/mb 0