kalmanshu.m

卡尔曼滤波算法的MATLAB源码程序

clc
clear
figure('color','w');
a=[0 1;0 -1];
b=[0 1]';
c=[1 0];
d=0;
[ap,bp,cp,dp]=augstate(a,b,c,d);
h=0.05;
t=[0:h:10];
[xorg,yorg]=step(ap,bp,cp,dp,1,t);
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;
bp=[b [0 0]'];
cp=c;
dp=[d 1];
[G,H]=c2d(ap,bp,h)%连续系统离散化
K=[0 0]';
xold=[0 0]';%%%状态初始值
pold=diag([10000 10000]);%%%%%%%%%%%%%%%%%p的初始值
xhatold=[u(1,2) 0]';%%%%%%%%%%估计初始值
Q=0.01;
R=2;
gamma=[0 1]';%量测噪声作用于系统的输出
[pp,gamma]=c2d(a,gamma,h);%得离散系统的误差耦合矩阵
y=zeros(length(t),5);
for jj=1:length(t)
    uplant=u(jj,:);
    xnew=G*xold+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]);plot(t,[xorg y(:,:)]);%全部
axes('pos',[0.52,0.56,0.35,.32]);plot(t,xorg(:,2),'r',t,y(:,1),'g',t,y(:,2),'k',t,y(:,4),'b');%状态1
axes('pos',[0.1,0.1,0.35,.32]);plot(t,xorg(:,3),'r',t, y(:,3),'k',t,y(:,5),'b');%%%%状态2
axes('pos',[0.52,0.1,0.35,.32]);plot(t,[xorg(:,[2 3])]);%原始系统状态输出