簡單的一個程序實現卡爾曼濾波驗證

話不說多說，本文假設的是一個以1米每秒勻速駕駛的小車，使用的octave軟件，咋們直接看程序實現：

z=(1:100); %觀測值
noise=randn(1,100); %方差為1的高斯噪聲
z=z+noise;
X=[0;0]; %狀態
P=[1 0;0 1]; %狀態協方差矩陣
F=[1 1;0 1]; %狀態轉移矩陣
Q=[0.0001,0;0 0.0001]; %狀態轉移協方差矩陣
H=[1 0]; %觀測矩陣
R=1; %觀測噪聲方差
figure;
hold on;
for i=1:100
 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_;
 
 plot(X(1),X(2));
endforxi

下面看看效果：

可以開出，只進行了幾次迭代，出的結果就保持在1左右很不錯了。

歡迎大家評論交流哈。