話不說多說，本文假設的是一個以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左右很不錯了。
歡迎大家評論交流哈。