22222.asv

卡尔曼滤波问题

clc
clear
T=0.1;
n=400;
t=1:20

F=[1,T,T*T/2,0,0,0;0,1,T,0,0,0;0,0,1,0,0,0;0,0,0,1,T,T*T/2;0,0,0,0,1,T;0,0,0,0,0,1];   
Gs=[T*T/4,T/2,1,0,0,0;0,0,0,T*T/4,T/2,0];

N1=randn(1,n);
N2=randn(1,n);
N3=randn(1,n);
W1=randn(1,n);
W2=randn(1,n);

Z=[N1+X(1,t);N2+X(2,t);N3+X(3,t)];          % input signal = Guass state noise + sin/cos
 
XX=[150;-5;-1.5;1;1;1];           % inital value
PP=[0.75,0.25;0.25,0.75];           % inital value
 
Q=[0.09,0;0,0.09];
R=[1,0,0;0,0.25,0;0,0,0.04];

X(k)=[x1(k);x2(k);x3(k);y1(k);y2(k);y3(k)];
for k=2:1:n
    Xs=XX
    P(k)=[F-K(k)*H(k)]*P(k-1)+Gs'*Q*Gs
    X(k)=F*X(k-1)+G*[W1;W2]
    Hk=P(k)*inv(P(k)+R);

    end
   
end
subplot(6,1,1);
plot(Z(1,t),'r');
hold on 
plot(X(1),'k')
axis([1 400 20 160])
subplot(6,1,2);
plot(X1,'b');
axis([1 400 20 160])
grid on;
 
subplot(6,1,1);
plot(Z(2,t),'r');
hold on 
plot(X(2),'k')
axis([1 400 20 160])
subplot(6,1,2);
plot(X2,'b');
axis([1 400 20 160])
grid on;subplot(6,1,1);
plot(Z(1,t),'r');
hold on 
plot(X(3),'k')
axis([1 400 20 160])
subplot(6,1,2);
plot(X3,'b');
axis([1 400 20 160])
grid on;