kalmanshugai2.m

比较卡尔曼滤波中状态方程的作用的ＭＡＴＬＡＢ源码

clc
clear
num=[1 1 -0.3 -0.06 0.008];
den=[1 -0.5 -0.49 0.125 0.06];
H=filt(num,den);
[Q e]=freqz(num,den,1500);
magQ=abs(Q);
subplot(3,1,1);plot(e/pi,magQ);
title('幅度响应');
[A,B,C,D]=tf2ss(num,den);%变换为离散系统状态方程

[ap,bp,cp,dp]=augstate(A,B,C,D);
xx=[0 0];
h=0.05;
t=[0:h:10];
[xorg,yorg]=lsim(ap,bp,cp,d,sin(t),t,xx);
u=zeros(length(t),2);
u(:,1)=0.1*rand(length(t),1);
u(:,2)=1.4142*rand(length(t),1);
m=mean(u(:,1));
u(:,1)=u(:,1)-m+1;
m=mean(u(:,2));
u(:,2)=u(:,2)-m;
%ap=a;
b1=[bp [0 0]'];
%cp=c;
dp=[d 1];
%[G,H]=c2d(ap,bp,h)%连续系统离散化
G=ap;
H=[bp [0 0]'];
K=[0 0]';
xold=[0 0]';%%%状态初始值
pold=diag([10000 10000]);%%%%%%%%%%%%%%%%%p的初始值
xhatold=[u(1,2) 0]';%%%%%%%%%%估计初始值
Q=0.01;
R=2;
%xxorg=xorg(:,[2 3]);
gamma=[0 1]';%量测噪声作用于系统的输出
%[pp,gamma]=c2d(a,gamma,h);%得离散系统的误差耦合矩阵
y=zeros(length(t),5);
for jj=1:length(t)
   yplant=yorg(jj,:);
    uplant=u(jj,:);
    xnew=G*xold+H*yplant'+H*uplant';%%状态方程
    y(jj,1)=(cp*xnew+dp*uplant')';%量测方程输出
   y(jj,2:3)=xnew';%状态方程输出
   %%%卡尔曼更新
    pstar=G*pold*G'+gamma*Q*gamma';
    K=pstar*cp'*inv(cp*pstar*cp'+R);
    xhatnew=(eye(2)-K*cp)*xnew+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]);
figure(1)
plot(t,xorg);%全部
figure(2)
%axes('pos',[0.52,0.56,0.35,.32]);
plot(t,yorg(:,1),'r',t,y(:,1),'g',t,y(:,2),'k',t,y(:,4),'b');%状态1
%axes('pos',[0.1,0.1,0.35,.32]);
figure(3)
plot(t,yorg(:,2),'r',t, y(:,3),'k',t,y(:,5),'b');%%%%状态2
%axes('pos',[0.52,0.1,0.35,.32]);
figure(4)
plot(t,[yorg(:,[1 2])]);%原始系统状态输出