dsp.m

Kalman滤波器

clear all;
sigma1=1;
sigma2=4;
N=100;
B=[1];
A=[1,-1.352,1.338,-0.662,0.240];
v1=sqrt(sigma1)*randn(1,N);
x=filter(B,A,v1);

v2=sqrt(sigma2)*randn(1,N);
y=x+v2;
F=[0,1,0,0;0,0,1,0;0,0,0,1;-0.24,0.662,-1.338,1.352];
Q1=zeros(4,4);
Q1(4,4)=sigma1;
C=[0,0,0,1];
Q2=sigma2;
gg=zeros(4,N);
[xx,gg]=kal(sigma1,F,Q1,C,Q2,y,N,4);

RR=zeros(5,5);
RR(1,:)=A;
RR(2,:)=[A(1),1+A(2),A(3),A(4),0];
RR(3,:)=[A(2),A(1)+A(3),1+A(4),0,0];
RR(4,:)=[A(3),A(2)+A(4),A(1),1,0];
RR(5,:)=A(5:-1:1);
t=[sigma1,0,0,0,0]';
r=inv(RR)*t;
Rx=zeros(4,4);
Rx(1,:)=r(1:1:4);
Rx(2,:)=[r(2),r(1),r(2),r(3)];
Rx(3,:)=[r(3),r(2),r(1),r(2)];
Rx(4,:)=[r(4),r(3),r(2),r(1)];
Ry=Rx+sigma2*eye(4);
rxd=r(1:1:4);
wo=inv(Ry)*rxd;
xx_wf=filter(wo,[1],y);
texts={'原信号x';'观测信号';'恢复信号'};
figure;
plot(x,'r');
hold on;
plot(y,'g-.');
hold on;
plot(xx,'b--');
hold on;
legend(texts);

figure;
plot(x,'r');
hold on;
plot(y,'g-.');
hold on;
plot(xx_wf,'b--');
hold on;
legend(texts);

figure;
plot(gg(1,:),'r-');
hold on;
plot(gg(2,:),'g-');
hold on;
plot(gg(3,:),'b-');
hold on;
plot(gg(4,:),'k-');
hold on;
legend({'g1';'g2';'g3';'g4'})