2014-07-14

Z=(1:100); %觀(guān)測(cè)值

noise=randn(1,100); %方差為1的高斯噪聲

Z=Z+noise;

X=[0; 0]; %狀態(tài)

P=[1 0; 01]; %狀態(tài)協(xié)方差矩陣

F=[1 1; 01]; %狀態(tài)轉(zhuǎn)移矩陣

Q=[0.0001, 0; 00.0001]; %狀態(tài)轉(zhuǎn)移協(xié)方差矩陣

H=[1 0]; %觀(guān)測(cè)矩陣

R=1; %觀(guān)測(cè)噪聲方差

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)); %畫(huà)點(diǎn)，橫軸表示位置，縱軸表示速度

end

愛(ài)華網(wǎng)本文地址 » http://www.klfzs.com/a/25101010/20420.html