z2guding_gai2.m

关于卡尔曼滤波器设计 Q R采用系统算出

clear all;
close all;

%产生信号
pi=3.14;
i=1:600;
s=square(4*pi*4*i);
sn=0.1*wgn(1,length(s),1);
v=0.01*wgn(1,length(s),1);
for l=2:1:600
b(l)=s(l-1)+sn(l-1);
y(l)=b(l)+v(l);
end
figure(1);
plot(s)
axis([0 length(s) -2 2]);
title('方波随机产生信号s(n)系统预测');

figure(2);
subplot(2,1,1);
axis([0 length(s) -2 2]);
plot(b)
title('含噪声的方波信号b(n)的时域图');
subplot(2,1,2);
f1=fft(b)
plot(abs(f1))
title('含噪方波信号b(n)的频谱')

figure(3);
subplot(2,1,1);
plot(y);
title('测量值');
subplot(2,1,2);
f3=fft(y)
plot(abs(f3));
title('测量值的频谱图')'

l1=sum(y);
x(1)=l1/600;
p(1)=((l1-x(1))*inv(l1-x(1)))/600; 


for l2=1:1:500
    y1(l2)=(y(l2+1)-y(l2));
    r1(l2)=var(y1);
    y2(l2)=(y(l2+2)-y(l2));
    r2(l2)=var(y2);
    R(l2)=(2*r1(l2)-r2(l2))/2;
    Q(l2)=(r2(l2)-r1(l2));
end

for k=2:1:500 
    x1(k)=x(k-1);
    p1(k)=p(k-1)+Q(k-1);
   
    kg(k)=p1(k)/(p1(k)+R(k));
    x(k)=x1(k)+kg(k)*(y(k)-x1(k));
    %p(k)=(1-kg(k))*p1(k);  
    y1(k)=y(k)-x(k);
    if k>=5
        for j=1:5
            y2(j)=y1(k-5+j);
        end
        p(k)=var(y2);
    end
    if k<5
        p(k)=var(y1);
    end
end


figure(4)
subplot(2,1,1);
axis([0 length(y) -2 2]);
plot(x1);
title('');
subplot(2,1,2);
f2=fft(x1);
plot(abs(f2));
figure(5)
subplot(2,1,1);
plot(kg);
title('kg');
%axis([0 length(u) -2 2]);
subplot(2,1,2);
plot(p1);
title('p1');
%axis([0 length(x) -2 2]);
figure(6);
subplot(2,1,1);
plot(Q);
title('Q');
subplot(2,1,2);
plot(R);
title('R');