一、簡介

1 卡爾曼濾波是什么
卡爾曼濾波適用于估計一個動態(tài)系統(tǒng)的最優(yōu)狀態(tài)。即便是觀測到的系統(tǒng)狀態(tài)參數(shù)含有噪聲，觀測值不準(zhǔn)確，卡爾曼濾波也能夠完成對狀態(tài)真實值的最優(yōu)估計。網(wǎng)上大多數(shù)的教程講到卡爾曼的數(shù)學(xué)公式推導(dǎo)，會讓人很頭疼，難以把握其中的主線和思想。所以我參考了國外一位學(xué)者的文章，講述卡爾曼濾波的工作原理，然后編寫了一個基于OpenCV的小程序給大家做一下說明。

2 卡爾曼濾波能做什么
假設(shè)我們手頭有一輛DIY的移動小車。這輛車的外形是這樣的：

這輛車可以在荒野移動，為了便于對它進行控制，需要知道它的位置以及移動速度。所以，建立一個向量，用來存儲小車的位置和速度

其實，一個系統(tǒng)的狀態(tài)有很多，選擇最關(guān)心的狀態(tài)來建立這個狀態(tài)向量是很重要的。例如，狀態(tài)還有水庫里面水位的高低、煉鋼廠高爐內(nèi)的溫度、平板電腦上面指尖觸碰屏幕的位置等等這些需要持續(xù)跟蹤的物理量。好了，回歸到正題，小車上面安裝了GPS傳感器，這個傳感器的精度是10米。但是如果小車行駛的荒野上面有河流和懸崖的話，10米的范圍就太大，很容易掉進去進而無法繼續(xù)工作。所以，單純靠GPS的定位是無法滿足需求的。另外，如果有人說小車本身接收操控著發(fā)送的運動指令，根據(jù)車輪所轉(zhuǎn)動過的圈數(shù)時能夠知道它走了多遠(yuǎn)，但是方向未知，并且在路上小車打滑車輪空轉(zhuǎn)的現(xiàn)象絕對是不可避免。所以，GPS以及車輪上面電機的碼盤等傳感器是間接地為我們提供了小車的信息，這些信息包含了很多的和不確定性。如果將所有這些信息綜合起來，是否能夠通過計算得到我們更想要的準(zhǔn)確信息呢？答案是可以的！

3 卡爾曼濾波的工作原理
1.先驗狀態(tài)估計
以之前我們創(chuàng)建的狀態(tài)變量為例，

下圖表示的是一個狀態(tài)空間圖，為了研究方便，假如小車在一條絕對筆直的線路上面行駛，其位置和速度的方向是確定的，不確定的是大小。

二、源代碼

clear; clc; %采樣點的個數(shù) N=228; %測試數(shù)據(jù)：緯度 latitude=load('C:\Users\lenovo\Desktop\基于MATLAB的運動軌跡預(yù)測，卡爾曼濾波實現(xiàn)\latitude.txt'); %真實維度值 lat=latitude; %卡爾曼濾波處理的狀態(tài)，即估計值 lat_kf=zeros(1,N); %測報值 lat_z=zeros(1,N); P=zeros(1,N); %初始緯度值 lat(1)=29.8131; %初始值的協(xié)方差 P(1)=0.09; %初始測報值 lat_z(1)=29.8027; %初始估計狀態(tài)。假設(shè)和初始測報值相同 lat_kf(1)=lat_z(1); %噪聲方差 %系統(tǒng)噪聲方差 Q=0.1; %測量噪聲方差 R=0.001; %方差決定噪聲大小 W=sqrt(Q)*randn(1,N); V=sqrt(R)*randn(1,N); %系統(tǒng)矩陣 F=1; G=1; H=1; %本系統(tǒng)狀態(tài)為1維 I=eye(1); %模擬緯度測報，并濾波 for k=2:N ? ?%隨時間推移，飛行緯度逐漸變化 ? ?%k時刻的真是緯度值是測報儀器不知道的，測報值可能是無限接近于真實值，但并不是真實值 ? ?%lat(k)=F*lat(k-1)+G*W(k-1); ? ?%緯度在k時刻的測報值 ? ?lat_z(k)=H*lat(k)+V(k); ? ?%kalman濾波 ? ?%有了k時刻的測報值lat_z(k)和k-1時刻的狀態(tài)，那么就可以進行濾波了 ? ?%狀態(tài)預(yù)測 ? ?lat_pre=F*lat_kf(k-1); ? ?%協(xié)方差預(yù)測 ? ?P_pre=F*P(k-1)*F'+Q; ? ?%計算卡爾曼增益 ? ?Kg=P_pre*inv(H*P_pre*H'+R); ? ?%新息 ? ?e=lat_z(k)-H*lat_pre; ? ?%狀態(tài)更新 ? ?lat_kf(k)=lat_pre+Kg*e; ? ?%協(xié)方差更新 ? ?P(k)=(I-Kg*H)*P_pre; end %計算誤差 %測量值與真實值之間的偏差 Err_Messure=zeros(1,N); %kalman估計與真實值之間的偏差 Err_Kalman=zeros(1,N); for k=1:N ? ?Err_Messure(k)=abs(lat_z(k)-lat(k)); ? ?Err_Kalman(k)=abs(lat_kf(k)-lat(k)); end t=1:N;

三、運行結(jié)果