kalman两变量滤波.asv

卡尔曼滤波问题

clc
clear
n=6000;
Fs=300;
t=1:n;
 g=0.6,a=0.06,d=0.36;
 Ts=1/0.6

Qk= 1.0
Rk= 0.85
A=[1  Ts;-d*Ts 1-a*Ts]
B=[g*Ts;-a*g*Ts]


X=[10*sin(2*t/Fs);10*cos(4*t/Fs)];
              
N1=randn(1,n);
N2=randn(1,n);
Z=[N1+X(1,t);N2];          % input signal = Guass state noise + sin/cos
 
XX=0.5*diag(ones(2,1));           % inital value
PP=[1 0 0 0.25];           % inital value
 
Q=0.02*diag(ones(2,1));
R=10*diag(ones(2,1));
 
A(1)=XX(1,1);
B(1)=XX(2,2);
I=diag(ones(2,1));
Xs=diag(zeros(2,1));
Ps=diag(zeros(2,1));
 
for k=2:1:n
    Xs=XX;
    Ps=PP+Q;
    Hk=Ps*inv(Ps+R);
    for i=1:1:2
        temp(:,i)=Hk*(Z(:,k)-Xs(:,i));
    end
    XX=Xs+temp;
    A(k)=XX(1,1);
    B(k)=XX(2,2);
    PP=(I-Hk).*Ps;
end
subplot(4,1,1);
plot(Z(1,t),'r');
hold on 
plot(X(1,t),'k')
axis([1 6000 -30 30])
subplot(4,1,2);
plot(A,'b');
axis([1 6000 -30 30])
grid on;
 
subplot(4,1,3);
plot(Z(2,t),'r');
hold on 
plot(X(2,t),'k')
axis([1 6000 -30 30])
subplot(4,1,4);
plot(B,'b');
axis([1 6000 -30 30])
grid on;