wanghai.m

详细介绍kalman滤波器在matlab中的应用！自己编的！申请加精！！！

clc  %清除命令区的历史信息
clear all %清除工作区的所有变量
 
a=[0   1;
   0  -1];
b=[0 2]'; 
c=[1 0];  
d=0;%构造连续系统矩阵

[ap,bp,cp,dp]=augstate(a,b,c,d);%把系统状态添加到输出向量中
h=0.05;%求取的时间间隔为0.05s
t=[0:h:10];%构造时间向量
[xorg,yorg]=step(ap,bp,cp,dp,1,t);%求取没有噪声时的系统在0~10s的输出

u=zeros(length(t),2);%这句话可要，可不要
%u一共有两列
u(:,1)=0.1*rand(length(t),1);%产生0~0.1之间的均匀分布数据
u(:,2)=1.4142*rand(length(t),1);%产生0~1.4142之间的均匀分布数据
m=mean(u(:,1));%求取u1的均值
%这儿u的第一列包含的信息由两部分，一个是本来的阶跃输入信号，式子中的1；
%两外一部分是加到输入上的随机噪声u1，即u(:,1)-m；
%能把噪声直接加到输入信号上，是因为例子中c矩阵跟H矩阵相同
u(:,1)=u(:,1)-m+1;
m=mean(u(:,2));
u(:,2)=u(:,2)-m;%产生噪声数据u2

ap=a;
bp=[b [0 0]'];
cp=c;
dp=[d 1];%把噪声u2也当成系统那个的一个输入，构造系统矩阵
[G,H]=c2d(ap,bp,h);%求取系统加入增广噪声输入后的系统离散化矩阵

K=[0 0]';%K阵的初始值
xold=[0 0]';%系统的状态初始值
pold=diag([10000 10000]);%系统状态协方差的初始值
xhatold=[u(1,2),0 ]'%系统状态输入值
Q=0.01;%噪声u1协方差
R=2;%噪声u2协方差
gamma=[0 1]';
[pp,gamma]=c2d(a,gamma,h);%例子中gamma与H相同

y=zeros(length(t),5);%存储系统输出和状态的数值

for jj=1:length(t)
    uplant=u(jj,:);
    xnew=G*xold+H*uplant';%加入噪声后的k+1时刻的系统状态值
    y(jj,1)=(cp*xnew+dp*uplant');%加入噪声后的k+1时刻的系统输出值
    y(jj,2:3)=xnew';%记录系统状态值
    
    %下面是（8.58）式
    pstar=G*pold*G'+gamma*Q*gamma';%预报的协方差
    K=pstar*cp'*inv(cp*pstar*cp'+R);
    xhatnew=(eye(2)-K*cp)*(G*xhatold+H*uplant')+K*y(jj,1);%估计的状态输出
    pnew=(eye(2)-K*cp*pstar);%估计的系统状态协方差
    
    y(jj,4:5)=xhatnew;
    xold=xnew;
    xhatold=xhatnew;
    pold=pnew;
end

axes('pos',[0.1,0.56,0.35,.32]);
plot(t,[xorg y(:,:)]);
axes('pos',[0.52,0.56,0.35,.32]);
plot(t,[xorg(:,2) y(:,[1 2 4])]);
legend('真实x1','系统实际输出','状态x1的实际值','状态x1的估计值');
title('系统状态x1的对比');
axes('pos',[0.1,0.1,0.35,.32]);
plot(t,[xorg(:,3) y(:,[3 5])]);
legend('真实x2','状态x2的实际值','状态x2的估计值');
title('系统状态x2的对比');
axes('pos',[0.52,0.1,0.35,.32]);plot(t,[xorg(:,[2 3])]);title('真实系统状态');