kalman_multi.m

卡尔曼滤波的MATLAB程序

clc;
clear all;
%clf;

n=6000;
Fs=300;
t=1:n;

X=[10*sin(t/Fs);10*cos(t/Fs)];
              
N=20*randn(1,n);
Z=[N+X(1,t);N+X(2,t)];          % input signal = Guass state noise + sin/cos

XX=9*diag(ones(2,1));           % inital value
PP=4*diag(ones(2,1));           % 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(2,1,1);
plot(A,'b');
hold on;
plot(Z(1,t),'r');
plot(X(1,t),'k')
grid on;

subplot(2,1,2);
plot(B,'b');
hold on;
plot(Z(2,t),'r');
plot(X(2,t),'k')
grid on;