线性卡尔曼滤波KF

线性卡尔曼滤波KF

clear;
clc;
N=100;
Z=(1:N);%观测值
noise=randn(1,N);%mean=0,方差为1的高斯噪声
Z=Z+noise;

X=[0;0];%初始状态
P=[1 0; 0 1];%状态协方差矩阵
F=[1 1; 0 1];%状态转移矩阵
Q=[0.0001 0; 0 0.0001];%状态转移协方差矩阵0.0001 0; 0 0.0001
H=[1 0];%观测矩阵
R=1;%观测噪声方差

figure;
hold on;

sz = [2,N];
res=zeros(sz);

for i=1:N
    X_=F*X;
    P_=F*P*F'+Q;
    K=P_*H'/(H*P_*H'+R);
    X=X_+K*(Z(i)-H*X_);
    P=(eye(2)-K*H)*P_;
    
    res(1,i) = X(1);
    res(2,i) = X(2);
    %plot(X(1), X(2),'r-');%横轴表示位置&纵轴表示速度
end
plot(res(1,:), res(2,:),'r-+');