% 基于_当前_统计模型的目标状态估计
 
% 主要靠 kalman滤波实现对物体运动轨迹的估计
 
%已知运动状态
 
%初始速度
 
600 1.匀速10s 2.加速度5，持续10s 3. 匀速5s
 
%采样周期 1s
 
% 程序代写&算法设计,联系qq:380238062，转载时请保留
 
clc
 
clear
 
close all
 
jiasudu=5;%加速度
 
R1=40; %观测噪声方差
 
R2=50;
 
V1=normrnd(0,R1,[25 1]); % A 观测噪声，服从高斯分布，零均值，方差为R1
 
V2=normrnd(0,R2,[25,1]);
 
for i=1:25
 
if
 
i<=10 %匀速
 
X(1,i)=600*i; %位置
 
X(2,i)=600; %速度
 
X(3,i)=0; %加速度
 
elseif
 
i<=20 %匀加速
 
X(1,i)=6000+600*(i-10)+0.5*jiasudu*(i-10)^2;
 
X(2,i)=600+jiasudu*(i-10);
 
X(3,i)=jiasudu;
 
else
 
%匀速
 
X(1,i)=14500+650*(i-20);
 
X(2,i)=650;
 
X(3,i)=0;
 
end
 
end
 
[weizhi1,P_P1,X_X1]=kalman_f(X,R1,V1,jiasudu); % kalman滤波
 
[weizhi2,P_P2,X_X2]=kalman_f(X,R2,V2,jiasudu);
 
for k=1:25
 
P1=P_P1{k};
 
P2=P_P2{k};
 
X1=X_X1{k};
 
X2=X_X2{k};
 
X_zuiyou(:,k)=pinv(P1+P2)*(P2*X1+P1*X2);
 
%最优加权估计 end
 
% 程序代写&算法设计,联系qq:380238062，转载时请保留
 
error1=X(1,:)'-weizhi1';%估计误差
 
error2=X(1,:)'-weizhi2';
 
error3=X(1,:)'-X_zuiyou(1,:)';
 
figure;
 
plot(error1);
 
hold on
 
plot(error2);
 
plot(error3,':');
 
text(3,2000,'最优加权误差 - - - - - - - - - - - - - -')
 
title('估计误差');
 
function [weizhi,P_P,X_X]=kalman_f(X,R1,V1,jiasudu);
 
% kalman滤波算法的实现
 
% 程序代写&算法设计,联系qq:380238062，转载时请保留
 
a=1/60;
 
fai=[1 1 (-1+a+exp(a))/(a^2);0 1 (1-exp(-a))/a;0 0
 
exp(-a)]; % Φ
 
U=[(-1+a/2+(1-exp(-a))/a)/a 1-(1-exp(-a))/a 1-exp(-a)]';
 
H=[1 0 0];
 
I=ones(3,3);
 
q11=(1-exp(-2*a)+2*a+2*(a^3)/3-2*(a^2)-4*a*exp(-a))/(2*a^5);
 
q12=(1+exp(-2*a)-2*exp(-a)+2*a*exp(-a)-2*a+a^2)/(2*a^4);
 
q13=(1-exp(-2*a)-2*a*exp(-a))/(2*a^3);
 
q22=(4*exp(-a)-3-exp(-2*a)+2*a)/(2*a^3);
 
q23=(exp(-2*a)+1-2*exp(-a))/(2*a^3);
 
q33=(1-exp(-2*a))/(2*a);
 
Q=[q11 q12 q13;
 
q12 q22
 
q23;
 
q13 q23
 
q33];
 
X0=[0 600 0]';
 
P0=1; %初始值
 
for k=1:25
 
if
 
i<=10
 
a_k=0;
 
elseif
 
i<=20
 
a_k=jiasudu;
 
else
 
a_k=0;
 
end
 
a_mean=X(1,k)/(k*k); %"当前"平均加速度
 
if
 
a_mean==0
 
sigma_a=1;
 
else
 
sigma_a=(4-pi)*(a_mean^2)/2;%“当前”加速度方差
 
end
 
Q=2*a*sigma_a*Q;
 
x_k_k_1=fai*X0+U*a_k;
 
p_k_k_1=fai*P0*(fai')+Q;
 
k_k=p_k_k_1*H'/(H*p_k_k_1*H'+R1);
 
y_k=H*X(:,k)+V1(k);
 
x_k_k=x_k_k_1+k_k*(y_k-H*x_k_k_1);
 
p_k_k=(I-k_k*H)*p_k_k_1;
 
X0=x_k_k;
 
P0=p_k_k;
 
P_P{k}=P0;
 
X_X{k}=x_k_k_1;
 
weizhi(k)=x_k_k_1(1);
 
end

引言
 
实际工程应用中，系统内部的状态难以使用传感器直接测量，
 在这种情况下，使用状态观测器估计系统实际状态是非常常见的做法
 以系统的状态空间表达式为例，下述状态观测器的设计方法：
 
举例说明
 

 给定线性定常系统
 
 
 
仿真测试
 

 x1状态的观测
 
 
 
 在模型输出加入随机噪声后：
 
 
 
 
 
线性扩张状态观测器详解 - SGH8224的文章 - 知乎
 https://zhuanlan.zhihu.com/p/108466027
 https://wenku.baidu.com/view/4709a15f804d2b160b4ec024.html
 

状态观测器是现代控制理论的重要组成部分[1-2],它是从系统状态空间方程出发,利用原系统的输入和输出信号获得原系统状态变量估值的方法,在传感器噪声处理、系统相移抑制以及高性能控制系统的设计中应用得非常普遍[2-3]。扩张状态观测器(ExtendedStateObserv-er)是自抗扰控制器(ActiveDisturbanceRejectionCon-troller)的核心部分[4-5],它不像通常的状态观测器如隆伯格观测器(LuenbergerObserver)[2]需要预先知道系统模型,也不关心系统是线性系统还是非线性系统,因此避免了使用过程中对模型的依赖[6]。本文通过Matlab[7]对扩张状态观测器进行数值仿真,给出并比较了ESO对二阶系统在线性参数和非线性参数下的观测效果,考察了系统存在高斯加性白噪声条件下的观测效果,并对观测结果进行了分析。1扩张状态观测器的基础理论对于常见的二阶系统,设其输入信号为u,输出信号为y,系统对输入信号的作用为b,此系统表达式记为:?x(2)+f(x(1),x)=buy=x(1)引入状态变量x1=x,x2=x(1)改写为状态空间表达式,并以Ts为采样周期进行离散化可得相应的离散状态方程:???x1(k+1)=x1(k)+Tsx2(k)x2(k+1)=x2(k)+Ts(-f(x2(k),x1(k))+bu(k))y(k)=x1(k)(2)依照文献[4]可用离散系统:?????e1(k)=z1(k)-y(k)z1(k)=z1(k-1)+Ts(z1(k)-01e(k))z2(k)=z2(k-1)+Ts(z3(k)-02fal(e1(k),0.5,)+bu(k))z3(k)=-03fal(e1(k),0.25,)(3)来观测原系统的状态变量。式(3)中:z1和z2分别为x1和x2的观测值;z3为系统扩张出来的状态变量;01,02,03和则为需要设定的参数。函数fal()定义为:fal(e,,)={e-1,|e||e|sign(e),|e|>(4)式中sign()是符号函数。以上推导过程见文献[4,8]。2扩张状态观测器的计算机仿真及结果比较分析仿真采用M语言[9]进行,仿真的系统选择为常见的二阶系统,其传递函数为[3]:G(s)=2s2+2s+2(5)引入状态变量并以Ts为周期离散化后可得离散状态空间方程:???x1(k)=x1(k-1)+Tsx2(k)x2(k)=x2(k-1)+Ts(-(2x2(k)+x1(k))+2u(k))y(k)=x1(k)(6)根据式(3)对该系统建立扩张状态观测器。仿真时,采样周期Ts设定为1ms,系统的阻尼系数设定为0.5,系统谐振频率f设定为3Hz。系统的输入信号正弦信号(幅度设定为1,频率设定为1Hz)。仿真结果做如下约定:,红色曲线为真实的系统状态,黑色曲线为观测出来的系统状态。仿真时设ESO中b为已知并保持不变,始终满足b=23,查看此时01,02,03为线性参数和非线性参数时的仿真结果。2.1无噪声时线性观测和非线性观测的观测效果线性估计时,可参考文献[10]进行,01=1Ts,02=1(3T)2s,03=2(8T)3s,fal()函数中的=1Ts;非线性估计时,参数需要做出适当的调整,目前只有通过试凑法进行,可以在线性估计的基础上进行,可选01=1Ts,02=1(3T)2s20,03=2(8)2T3s202,fal()函数中的=1Ts2,结果如图1、图2所示。图1无噪声时正弦信号的线性观测效果从仿真结果可看出,线性观测的效果略好于非线性观测的效果,而采用非线性估计时,E

Matlab之Kalman：用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法
 
 
 
 
 
目录
 
问题探究
 
卡尔曼滤波及数据滤波
 
代码实现
 

 
 
 
 
 
 
问题探究
 
用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
 
 
 
 
 
 
 
卡尔曼滤波及数据滤波
 
       卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
        斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。
        数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用
 
 
 
 
 
 
代码实现
 
clear all;
clc;
close all;


N=2000;
gv=0.0332;
m=500;
a=[-1.6 1.46 -0.616 0.1525];
Q2=0.005;%观测噪声方差
w=zeros(m*4,N);
alpher=zeros(m,N);

for j=1:m
    
    
v=randn(1,N)*sqrt(gv);

u=filter(1,a,v);


F=eye(4,4);
d=u;
C=zeros(4,N);
for i=5:N
    C(:,i)=[u(i-1);u(i-2);u(i-3);u(i-4)];
end
C(:,1)=[0;0;0;0];
C(:,2)=[d(1);0;0;0];
C(:,3)=[d(2);d(2);0;0];
C(:,4)=[d(3);d(2);d(1);0];

p_esti=eye(4,4);%状态误差自相关矩阵

for i=2:N
    w([4*j-3,4*j-2,4*j-1,4*j],i)=w([4*j-3,4*j-2,4*j-1,4*j],i-1);%一步预测误差
    alpher(j,i)=d(i)-C(:,i)'*w([4*j-3,4*j-2,4*j-1,4*j],i).'';%计算信息过程
    p_pre=p_esti;%一步预测误差自相关矩阵
    A=C(:,i)'*p_pre*C(:,i)+Q2;%新息过程自相关矩阵
    k=p_pre*C(:,i)/A;
    w([4*j-3,4*j-2,4*j-1,4*j],i)=w([4*j-3,4*j-2,4*j-1,4*j],i)+k*alpher(j,i);%状态估计
    p_esti=p_pre-k*C(:,i)'*p_pre;%状态估计误差自相关矩阵
end

end


MSE=sum(alpher.^2)/m;
MSE=MSE/max(MSE);
MSE=10*log10(MSE);
figure(1)
plot(MSE);
title(' 均方误差变化曲线');
xlabel('迭代次数n');ylabel('MSE');

w_aver=zeros(4,N);
for i=1:m
    w_aver(1,:)=w_aver(1,:)+w(4*i-3,:);
    w_aver(2,:)=w_aver(2,:)+w(4*i-2,:);
    w_aver(3,:)=w_aver(3,:)+w(4*i-1,:);
    w_aver(4,:)=w_aver(4,:)+w(4*i,:);
end

w_aver=w_aver/m;
figure(2) 
hold on;
plot(w_aver(1,:),'b');
plot(w_aver(2,:),'b');
plot(w_aver(3,:),'b');
plot(w_aver(4,:),'b');
title('权向量的估计');
xlabel('迭代次数n');ylabel('抽头权值');
hold off;