線性卡爾曼濾波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-+');