本文介绍了关于如何在matlab中实现课本中所学的状态反馈控制。状态反馈控制率的设计为u=-Kx+v;
假设一个系统，我们只知道它的输入u和输出y=x1，而其他状态在现实中是不可测量的，那么我们如何进行状态反馈的设计?这就需要引入状态观测器来对无法测量的状态进行估计。
分为以下几个步骤：
验证系统可观测性可控性
观测器程序设计
极点配置
仿真模型图如上所示。状态空间描述如下：
首先判定系统是否可观测可控：
观测器设计程序:
注意：这里采用了LESO的参数整定方法。将观测器的极点统一配置在-w，然后选取带宽的值
已知ABC矩阵，那么设带宽为160 则选取合适的L阵满足条件，使得观测器的误差是稳定的。
L的选取计算方法：
最终得到L矩阵，观测器设计完毕。
极点配置部分：
可控性验证：
A=[-0.044 66666 0;-1 0 0.01;0 -65217 -21.7];
B=[0;0;2174];
Co=ctrb(A,B)
rank(Co)
极点配置程序：
A=[-0.044 66666 0;-1 0 0.01;0 -65217 -21.7];
B=[0;0;2174];
J=[-10;-10;-14.5];
eig(A)
K=acker(A,B,J)
eig(A-B*K)
通过acker函数计算出K阵，极点配置控制率设计完毕。
实际出现问题总结：
出现不稳定问题，L有问题。极点配置不能离虚轴太近
一开始总是末尾不稳定，但是把极点调到-100左右解决问题 
虚部也不能太大，会有稳态误差。但是极点可以选的很远三个极点之间的关系影响了稳态误差，通过调整他们的间距，解决稳态误差问题最终-100 -100 -145 
 观测器跟踪存在稳态误差，如何消去稳态误差? 已解决:观测器方程有误，正确形式如下：
仿真结果如下，分别对阶跃曲线和正弦曲线进行跟踪:
可以看到 在1.5s加入了正弦扰动，仍能得到很好的抑制。
[1] 陈增强, 孙明玮, 杨瑞光. 线性自抗扰控制器的稳定性研究[J]. 自动化学报, 2013, 39(5):574-580.
[2 ]Gao Z . [IEEE 2003 American Control Conference - Denver, CO, USA (4-6 June 2003)] Proceedings of the 2003 American Control Conference, 2003. - Scaling and bandwidth-parameterization based controller tuning[C]// American Control Conference, 2003. Proceedings of the 2003. IEEE, 2003:4989-4996.
二周目：
时隔许久再次回来看这篇文发现，这篇并没有涉及ESO，只是基础的龙贝格观测器（全维状态观测器），寻找一个L阵使得A-LC是赫尔维茨即可，并不需要严格的整定策略。
而LESO其实就是全维观测器多了一阶而已，整定方法说白了就是把A-LC极点配在同一处，找到对应的那个L就ok。
A-LC是不是和A-BK形式很像？我们做状态反馈会求K阵，那么这里要求L阵，其实也是一样的。很简单，只要转置一下就可以了。sI-（A-LC）进行转置后行列式是不变的，所以可以转置为sI-（A‘-C'L’）L‘就相当于K
后来又做了一个四阶系统的LESO的仿真，发现x1~x3都可以精确估计，x4却一直是0，反复检查不知道错在哪。结果发现不是我式子写错了，是观测器增益不够大，一个四阶系统按照LESO的整定策略数值能上百万。。（其实不夸张，因为最后一项出现了4次方，轻轻松松就上百万。。）所以，它看上去是0不代表就是0，是上升速度实在太慢了（只仿真了5秒）所以，理论拿来做一下仿真就会发现很多不知道的问题，就算极点在左边，实际也不能满足要求，还要考虑快速性、噪声放大等问题。
至于出现的末尾发散问题，并不只是你看到的只在末尾发散，实际上从一开始就在发散了，指数的原因，后面的数太大了前面看不出来而已。俗称指数爆炸。
matlab编程这一块内容，使用s-function写模块是很方便的，定义好输入输出，中间再做运算就可以了。而且方便去改参数，只是matlab实在是太慢了，增益改大一点计算速度明显变慢。

本文介绍了关于如何在matlab中实现课本中所学的状态反馈控制。状态反馈控制率的设计为u=-Kx+v;
假设一个系统，我们只知道它的输入u和输出y=x1，而其他状态在现实中是不可测量的，那么我们如何进行状态反馈的设计?这就需要引入状态观测器来对无法测量的状态进行估计。
分为以下几个步骤：
验证系统可观测性可控性
观测器程序设计
极点配置
仿真模型图如上所示。状态空间描述如下：
首先判定系统是否可观测可控：
观测器设计程序:
注意：这里采用了LESO的参数整定方法。将观测器的极点统一配置在-w，然后选取带宽的值
已知ABC矩阵，那么设带宽为160 则选取合适的L阵满足条件，使得观测器的误差是稳定的。
L的选取计算方法：
最终得到L矩阵，观测器设计完毕。
极点配置部分：
可控性验证：
A=[-0.044 66666 0;-1 0 0.01;0 -65217 -21.7];
B=[0;0;2174];
Co=ctrb(A,B)
rank(Co)
极点配置程序：
A=[-0.044 66666 0;-1 0 0.01;0 -65217 -21.7];
B=[0;0;2174];
J=[-10;-10;-14.5];
eig(A)
K=acker(A,B,J)
eig(A-B*K)
通过acker函数计算出K阵，极点配置控制率设计完毕。
实际出现问题总结：
出现不稳定问题，L有问题。极点配置不能离虚轴太近
一开始总是末尾不稳定，但是把极点调到-100左右解决问题 
虚部也不能太大，会有稳态误差。但是极点可以选的很远三个极点之间的关系影响了稳态误差，通过调整他们的间距，解决稳态误差问题最终-100 -100 -145 
 观测器跟踪存在稳态误差，如何消去稳态误差? 已解决:观测器方程有误，正确形式如下：
仿真结果如下，分别对阶跃曲线和正弦曲线进行跟踪:
可以看到 在1.5s加入了正弦扰动，仍能得到很好的抑制。
[1] 陈增强, 孙明玮, 杨瑞光. 线性自抗扰控制器的稳定性研究[J]. 自动化学报, 2013, 39(5):574-580.
[2 ]Gao Z . [IEEE 2003 American Control Conference - Denver, CO, USA (4-6 June 2003)] Proceedings of the 2003 American Control Conference, 2003. - Scaling and bandwidth-parameterization based controller tuning[C]// American Control Conference, 2003. Proceedings of the 2003. IEEE, 2003:4989-4996.
二周目：
时隔许久再次回来看这篇文发现，这篇并没有涉及ESO，只是基础的龙贝格观测器（全维状态观测器），寻找一个L阵使得A-LC是赫尔维茨即可，并不需要严格的整定策略。
而LESO其实就是全维观测器多了一阶而已，整定方法说白了就是把A-LC极点配在同一处，找到对应的那个L就ok。
A-LC是不是和A-BK形式很像？我们做状态反馈会求K阵，那么这里要求L阵，其实也是一样的。很简单，只要转置一下就可以了。sI-（A-LC）进行转置后行列式是不变的，所以可以转置为sI-（A‘-C'L’）L‘就相当于K
后来又做了一个四阶系统的LESO的仿真，发现x1~x3都可以精确估计，x4却一直是0，反复检查不知道错在哪。结果发现不是我式子写错了，是观测器增益不够大，一个四阶系统按照LESO的整定策略数值能上百万。。（其实不夸张，因为最后一项出现了4次方，轻轻松松就上百万。。）所以，它看上去是0不代表就是0，是上升速度实在太慢了（只仿真了5秒）所以，理论拿来做一下仿真就会发现很多不知道的问题，就算极点在左边，实际也不能满足要求，还要考虑快速性、噪声放大等问题。
至于出现的末尾发散问题，并不只是你看到的只在末尾发散，实际上从一开始就在发散了，指数的原因，后面的数太大了前面看不出来而已。俗称指数爆炸。
matlab编程这一块内容，使用s-function写模块是很方便的，定义好输入输出，中间再做运算就可以了。而且方便去改参数，只是matlab实在是太慢了，增益改大一点计算速度明显变慢。

转载 张文宇的博客   https://blog.csdn.net/zhangwenyu111/article/details/17034813

卡尔曼滤波是一个很常用的滤波算法，与维纳滤波相比有很多长处。这里我们把Kalman Filter简称为KF。KF的基本思想是：采用信号、噪声、状态空间模型，利用前一时刻的状态最优估计值及其误差方差估计和现时刻的量测值来更新对状态变量的估计，求出现在时刻的最优估计值。

说白点就是对现在时刻的估计（可能是同时估计好几个变量）是取决于前一时刻估计误差和现在时刻的某个观测值。通过不断的预测和实测来修正自己的估计值，最后达到一个理想的平稳状态。

卡尔曼滤波的具体原理和推导过程，有点复杂。某些细节我也没弄明白，不过这并不影响我们写程序和使用它。如果你看了我写的这个小分享，表明你对卡尔曼滤波有一定的了解了，否则你也不会看这种玩意。如果真的不了解，那可以查找下相关资料。

卡尔曼滤波的5大公式：



这五个公式公式可要注意他们的下标了，下面我们从上至下依次说下这五个公式的意思

第一个公式就是说我们研究的变量在这一时刻与上一时刻的具体关系。那个Fai矩阵就是系数对应关系。可以看到这里就把卡尔曼滤波的使用范围限定在线性系统中了，基本的卡尔曼滤波器是不能用于非线性系统的。

第二个公式叫方差先验估计，说的这么文艺，其实就是计算估计值和实际值的方差大小，以评估现在的估计的误差是不是合适。

第三个公式叫增益矩阵，前面已经说过，卡尔曼滤波根据当前观测变量的变化趋势来估计一时刻变量的值，这个增益矩阵就是指我们定的这个变化的“度”的大小。

第四个就是求要估计的值了。

第五个是修正估计值与实际值的方差。

这5个公式是卡尔曼滤波的关键，下面就结合一个实例在说下卡尔曼滤波的程序如何写~交大的同学，如果你们选了最优估计这门课，然后看到这个，嘿嘿，便宜你们了。

 

应用：卡尔曼滤波在目标测量中的应用

某火箭在空中沿直线做匀加速飞行，加速度为  ，发动机推力的波动一定会引起加速度波动，雷达以间隔时间不断测量该火箭距离远点的距离  ，由于雷达的测量噪声使得火箭距离测量值   中含有噪声。现在欲通过雷达对火箭距离的测量数据求得火箭的真实距离、速度和加速度。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%     设置初始化信息

%  N：设置卡尔曼滤波器追踪点数

%  r：设置估计变量个数，这里r=3

%  s：被追踪的火箭的距离，初始值为1000m

%  v：火箭的速度，初始值为50m/s

%  a：火箭的加速度，初始值为20m/s2，此时加速度默认为不变

%  T: 雷达的扫描间隔，此时设为1秒

%  wt: 系统噪声，方差为20

%  vt: 量测噪声，方差为16

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear all;

close all;

N = 100;

r = 3;

t = 1:1:N;

T = 1;

s = zeros(1,N);

v = zeros(1,N);

a = zeros(1,N);

a(t) = 20;

s0 = 1000;

v0 = 50;

for n = 1:N

    v(n) = v0 + a(n)*n;

    s(n) = 1000+v0*n+0.5*a(n)*n*n;

end

wt = randn(1,N);

wt = sqrt(4)*wt./std(wt);

s = s + wt;

v = v + wt;

a(t) = a(t) + wt(t);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 卡尔曼滤波部分，继承之前初始化变量

% A：转移矩阵

% H：量测矩阵

% Qk：系统噪声矩阵

% Rk：量测噪声矩阵

% P0：均方误差矩阵初始值

% Y：火箭的状态矩阵，由k_s，k_v,k_a组成

% Y0：状态矩阵的初始值

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Y0 = [900 0 0]';

Y = [Y0 zeros(r,N-1)];

A = [1 1 0;0 1 1;0 0 1];

H = [1 0 0];

Qk = [0 0 0;0 0 0;0 0 20];

Rk = 16;

P0 = [30 0 0;0 20 0;0 0 10];

P1 = P0;

P2 = zeros(r,r);

for k = 1:N

    Y(:,k) = A*Y(:,k);

    P2 = A*P1*A'+Qk;

    Kk = P2*H'*inv(H*P2*H'+Rk);

    Y(:,k+1) = Y(:,k)+Kk*(s(:,k)-H*Y(:,k));

    P1 = (eye(r,r)-Kk*H)*P2;

end

k_s = Y(1,:);

k_v = Y(2,:);

k_a = Y(3,:);

subplot(3,1,1);

plot(t,s(t),'-',t,k_s(t),'o');

title('距离');

legend('实际值','估计值');

xlabel('t');

ylabel('s');

subplot(3,1,2);

plot(t,v(t),t,k_v(t),'+');

title('速度');

legend('实际值','估计值');

xlabel('t');

ylabel('v');

subplot(3,1,3);

plot(t,a(t),t,k_a(t),'-x');

title('加速度');

legend('实际值','估计值');

xlabel('t');

ylabel('a');

axis([0,N,0,30]);

	今天写了个卡尔曼滤波的小程序，希望对有需要的同学有点帮助。

卡尔曼滤波是一个很常用的滤波算法，与维纳滤波相比有很多长处。这里我们把Kalman Filter简称为KF。KF的基本思想是：采用信号、噪声、状态空间模型，利用前一时刻的状态最优估计值及其误差方差估计和现时刻的量测值来更新对状态变量的估计，求出现在时刻的最优估计值。
说白点就是对现在时刻的估计（可能是同时估计好几个变量）是取决于前一时刻估计误差和现在时刻的某个观测值。通过不断的预测和实测来修正自己的估计值，最后达到一个理想的平稳状态。
卡尔曼滤波的具体原理和推导过程，有点复杂。某些细节我也没弄明白，不过这并不影响我们写程序和使用它。如果你看了我写的这个小分享，表明你对卡尔曼滤波有一定的了解了，否则你也不会看这种玩意。如果真的不了解，那可以查找下相关资料。
卡尔曼滤波的5大公式：


这五个公式公式可要注意他们的下标了，下面我们从上至下依次说下这五个公式的意思
第一个公式就是说我们研究的变量在这一时刻与上一时刻的具体关系。那个Fai矩阵就是系数对应关系。可以看到这里就把卡尔曼滤波的使用范围限定在线性系统中了，基本的卡尔曼滤波器是不能用于非线性系统的。
第二个公式叫方差先验估计，说的这么文艺，其实就是计算估计值和实际值的方差大小，以评估现在的估计的误差是不是合适。
第三个公式叫增益矩阵，前面已经说过，卡尔曼滤波根据当前观测变量的变化趋势来估计一时刻变量的值，这个增益矩阵就是指我们定的这个变化的“度”的大小。
第四个就是求要估计的值了。
第五个是修正估计值与实际值的方差。
这5个公式是卡尔曼滤波的关键，下面就结合一个实例在说下卡尔曼滤波的程序如何写~交大的同学，如果你们选了最优估计这门课，然后看到这个，嘿嘿，便宜你们了。

应用：卡尔曼滤波在目标测量中的应用
某火箭在空中沿直线做匀加速飞行，加速度为  ，发动机推力的波动一定会引起加速度波动，雷达以间隔时间不断测量该火箭距离远点的距离  ，由于雷达的测量噪声使得火箭距离测量值   中含有噪声。现在欲通过雷达对火箭距离的测量数据求得火箭的真实距离、速度和加速度。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     设置初始化信息
%  N：设置卡尔曼滤波器追踪点数
%  r：设置估计变量个数，这里r=3
%  s：被追踪的火箭的距离，初始值为1000m
%  v：火箭的速度，初始值为50m/s
%  a：火箭的加速度，初始值为20m/s2，此时加速度默认为不变
%  T: 雷达的扫描间隔，此时设为1秒
%  wt: 系统噪声，方差为20
%  vt: 量测噪声，方差为16
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
close all;
N = 100;
r = 3;
t = 1:1:N;
T = 1;
s = zeros(1,N);
v = zeros(1,N);
a = zeros(1,N);
a(t) = 20;
s0 = 1000;
v0 = 50;
for n = 1:N
    v(n) = v0 + a(n)*n;
    s(n) = 1000+v0*n+0.5*a(n)*n*n;
end
wt = randn(1,N);
wt = sqrt(4)*wt./std(wt);
s = s + wt;
v = v + wt;
a(t) = a(t) + wt(t);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡尔曼滤波部分，继承之前初始化变量
% A：转移矩阵
% H：量测矩阵
% Qk：系统噪声矩阵
% Rk：量测噪声矩阵
% P0：均方误差矩阵初始值
% Y：火箭的状态矩阵，由k_s，k_v,k_a组成
% Y0：状态矩阵的初始值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Y0 = [900 0 0]';
Y = [Y0 zeros(r,N-1)];
A = [1 1 0;0 1 1;0 0 1];
H = [1 0 0];
Qk = [0 0 0;0 0 0;0 0 20];
Rk = 16;
P0 = [30 0 0;0 20 0;0 0 10];
P1 = P0;
P2 = zeros(r,r);
for k = 1:N
    Y(:,k) = A*Y(:,k);
    P2 = A*P1*A'+Qk;
    Kk = P2*H'*inv(H*P2*H'+Rk);
    Y(:,k+1) = Y(:,k)+Kk*(s(:,k)-H*Y(:,k));
    P1 = (eye(r,r)-Kk*H)*P2;
end
k_s = Y(1,:);
k_v = Y(2,:);
k_a = Y(3,:);
subplot(3,1,1);
plot(t,s(t),'-',t,k_s(t),'o');
title('距离');
legend('实际值','估计值');
xlabel('t');
ylabel('s');
subplot(3,1,2);
plot(t,v(t),t,k_v(t),'+');
title('速度');
legend('实际值','估计值');
xlabel('t');
ylabel('v');
subplot(3,1,3);
plot(t,a(t),t,k_a(t),'-x');
title('加速度');
legend('实际值','估计值');
xlabel('t');
ylabel('a');
axis([0,N,0,30]);
具体细节不讲了。运行后，就得到我们想要的结果了~







一个小总结，希望对你有帮助。