卡爾曼濾波實(shí)例——均加速運(yùn)動(dòng)的卡爾曼濾波
此系列（一）對(duì)卡爾曼濾波的原理進(jìn)行了簡(jiǎn)單的闡述，總結(jié)了卡爾曼濾波的兩大過(guò)程：預(yù)測(cè)和更新。接下來(lái)舉例對(duì)卡爾曼濾波的使用進(jìn)行介紹，加深對(duì)卡爾曼濾波的理解。
均加速小車(chē)的狀態(tài)估計(jì)（位置和速度）
1.場(chǎng)景介紹
如上圖所示，可知小車(chē)的初始速度為0，初始位置也為0，小車(chē)向前的加速度為1,小車(chē)感知自身狀態(tài)（位置）是通過(guò)GPS完成的，而GPS的精度較低，其探測(cè)精度的方差為10；且在這個(gè)系統(tǒng)中還存在著其他影響小車(chē)運(yùn)動(dòng)的因素，如風(fēng)阻，地面摩擦力和其他環(huán)境因素等等，所以我們不能完全通過(guò)GPS的測(cè)量完成對(duì)小車(chē)的定位，這種情況下，卡爾曼濾波閃亮登場(chǎng)。
為了完成對(duì)小車(chē)的定位（狀態(tài)估計(jì)），首先我們要確定小車(chē)運(yùn)動(dòng)的狀態(tài)方程，分析整個(gè)運(yùn)動(dòng)系統(tǒng)后，我們得知
假定每次估計(jì)的時(shí)間差值為1,帶入加速度a=1,其狀態(tài)方程表達(dá)式如下
由卡爾曼濾波公式（1）確定
觀測(cè)方程為
故
接下來(lái)利用卡爾曼濾波公式（2）求解狀態(tài)方程的協(xié)方差矩陣預(yù)測(cè)值
令R為
再利用卡爾曼濾波公式（3），求解卡爾曼濾波的增益系數(shù)矩陣
這里令Q=10即可。
利用卡爾曼濾波公式(4)對(duì)當(dāng)前狀態(tài)進(jìn)行最優(yōu)估計(jì)，
繼續(xù)更新?tīng)顟B(tài)方程協(xié)方差矩陣的估計(jì)值，為下一次狀態(tài)估計(jì)做準(zhǔn)備。
以上過(guò)程，完成了一次卡爾曼濾波狀態(tài)估計(jì)。
2.MATLAB仿真
根據(jù)場(chǎng)景介紹描述，若不使用卡爾曼濾波的方法，單獨(dú)使用GPS數(shù)據(jù)對(duì)小車(chē)位置進(jìn)行定位，其在前30s的位置信息如下圖所示
橫軸表示時(shí)間變化，豎軸表示小車(chē)定位數(shù)據(jù)。
使用卡爾曼濾波后的前30s位置信息如下圖所示。
兩者對(duì)比圖如下
從對(duì)比圖可以看出，使用卡爾曼濾波后位置信息中的噪聲數(shù)據(jù)得到明顯遏制，位置信息變化更為平滑。文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-612962.html
3.MATLAB仿真代碼
上述仿真模型代碼如下
Z=1:30;
noise = 10randn(1,30);
Z=Z.^2;
Z=Z+noise;
plot(Z);
X=[0; 0]; %初始狀態(tài)X
P=[1 0; 0 1];%初始狀態(tài)協(xié)方差矩陣
A=[1 1; 0 1];%狀態(tài)轉(zhuǎn)移矩陣
B=[0.5 1];
a=1;
R=[0.01 0; 0 0.01];%狀態(tài)轉(zhuǎn)移協(xié)方差
C=[1 0];%觀測(cè)矩陣
Q=10;
% figure;
hold on;
for i=1:30
X_=AX+Ba;
P_=APA’+R;
K=P_C’/(CP_C’+Q);
X=X_ + K(Z(i)-CX_);
P=(eye(2)-K*C)*P_;
plot(i,X(1),‘.’);
% pause(1)
end文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-612962.html
第一個(gè)實(shí)例介紹完成，后續(xù)會(huì)接著對(duì)拓展卡爾曼濾波等卡爾曼濾波變種進(jìn)行研究。
覺(jué)得不錯(cuò)的話(huà)，點(diǎn)個(gè)贊鼓勵(lì)一下哦
到了這里，關(guān)于卡爾曼濾波系列_實(shí)例（二）均加速運(yùn)動(dòng)的卡爾曼濾波的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
