文章目錄

• 直觀理解
• 卡爾曼濾波怎么做的？
• 卡爾曼濾波Python代碼實踐

原文鏈接:https://zhuanlan.zhihu.com/p/77327349

直觀理解

**首先卡爾曼濾波要解決的問題是什么？**我以我軍發(fā)射一枚導彈攻擊敵方某固定位置目標為例（搞科技的總要點情懷，老是講啥小車運動，溫度計這些就太low）。導彈需要每隔一秒開雷達測量下離目標的距離。然后由于雷達有誤差，所以需要融合自己上個時刻的位置、速度等信息來更準確的確定當前時刻離目標的距離。這就是卡爾曼濾波需要解決的事

從直觀理解卡爾曼濾波是怎么解決這個問題的呢？

首先導彈已知“當前這秒雷達測量的導彈離目標的距離（我們稱它為觀測值，比如雷達直接測量導彈離目標距離7m）”，“上個時刻導彈離目標距離”和“導彈自己當前時刻的速度”這三個數(shù)據(jù)。而根據(jù)“上個時刻導彈離目標距離”和“導彈自己當前時刻的速度”可以估算出當前導彈離目標的距離（我們稱它為估計值）。比如：上一秒離目標10m，速度是4m/s，那么現(xiàn)在這秒估計就離目標距離是6m。這個速度數(shù)據(jù)可以從傳感器里面讀取也可以從發(fā)動機那獲得。（根據(jù)速度估計導彈距離這個計算叫做移動模型建模，想了解移動模型與觀測值和濾波算法之間的聯(lián)系可以看這篇文章《機器人移動模型：根據(jù)控制命令預測機器人位置》）

那么問題來了，導彈離目標的距離現(xiàn)在既有個觀測值7m，又有個估計值6m。到底相信哪個？單純相信觀測值萬一雷達被敵方干擾了呢？單純相信估計值那么萬一上個時刻的距離估計值或者速度不準呢？所以，我們要根據(jù)觀測值和估計值的準確度來得到最終導彈離目標的距離估計值。準確度高的就最終結果比重高，準確度低就占比低。如果雷達測量的那個7m準確度是49%，根據(jù)速度估計出的那個6m準確度是1%，那么最終的距離估計結果就是
（這些數(shù)據(jù)大家有直觀理解就好，文末會給出計算方法）

卡爾曼濾波怎么做的？

我們先回顧總結下直觀理解中是怎么做的。

根據(jù)上一秒導彈的位置 和 導彈的的速度估計出當前時刻導彈的位置粗略估計值。
將雷達測得導彈位置測量值和我們計算出的導彈位置粗略估計值根據(jù)這兩種數(shù)據(jù)可信度來進行線性加權和得到準確的導彈位置估計值。
在前面我們也提到了導彈的位置和雷達測量值都是有誤差的。所以卡爾曼想用概率來衡量數(shù)據(jù)的可信度。

比如：雷達測量的數(shù)據(jù)它就不只是一個數(shù)字了。而是說測量發(fā)現(xiàn)導彈有0.8的概率在7m那個位置，有0.1的概率在7.2m那個位置。有0.1的概率在6.9m那個位置。這些數(shù)據(jù)就叫做概率分布。概率分布的意思就是很多個值還有它們各自出現(xiàn)的概率多大所組成的數(shù)據(jù)就叫做概率分布。

卡爾曼認為導彈速度、導彈位置、雷達測距的測量值這些都服從正態(tài)分布（對就是高中學的那個正態(tài)分布）。這是啥意思呢？比如下面這個圖是講上個時刻導彈的位置的概率分布，從圖中看出它在10m（橫軸）那個位置的縱坐標最高所以概率最大。它在其他地方的概率相對小一點。

卡爾曼濾波Python代碼實踐

這個實踐的已知量是導彈做水平運動，已知導彈每個時刻的速度dv,和速度測量儀器的標準差是v_std，還已知每個時刻用GPS測量出導彈位置position_noise以及GPS的方差是predict_var。（注意標準差的平方是方差，不用覺得奇怪）
我們需要用卡爾曼濾波根據(jù)這些信息獲得融合兩種傳感器后的位置信息position_predict

#coding=utf-8import numpy as np #-----------------------------生成無誤差的GPS坐標點------------------------------------------ # 模擬數(shù)據(jù) t = np.linspace(1,100,100)#在1-100之間，按照同樣的間隔生成100個數(shù)據(jù) a = 0.5 #加速度 position = (a * t**2)/2 #根據(jù)加速度獲取位置，模擬GPS獲取的位置 #給位置添加一個具有正態(tài)分布的誤差 # random.normal 正態(tài)分布 均值，標準差。樣本數(shù)（t.shape[0]獲取和第一維度數(shù)一樣的樣本數(shù)） #---------------------------生成帶高斯分布誤差的GPS測量值誤差-------------------------- position_noise = position+np.random.normal(0,120,size=(t.shape[0])) import matplotlib.pyplot as plt plt.plot(t,position,label='truth position') plt.plot(t,position_noise,label='only use measured position')#----------------------------------------設定初始值----------------------------------------------------------------- # 初始的估計導彈的位置就直接用GPS測量的位置 predicts = [position_noise[0]] position_predict = predicts[0] #------------------------------------------設定方差------------------------------------------------------------------- predict_var = 0 #預測方差 odo_var = 120**2 #這是我們自己設定的位置測量儀器的方差，越大則測量值占比越低，位置誤差方差 v_std = 50 # 速度標準差 for i in range(1,t.shape[0]):#循環(huán)從1開始到樣本數(shù)前一個結束dv = (position[i]-position[i-1]) + np.random.normal(0,50) # 模擬從IMU讀取出的速度#------------------預測--------------------position_predict = position_predict + dv # 利用上個時刻的位置和速度預測當前位置predict_var += v_std**2 # 預測速度方差# ---------------更新------------------------#根據(jù)權重更新位置預測position_predict = position_predict*odo_var/(predict_var + odo_var)+position_noise[i]*predict_var/(predict_var + odo_var)predict_var = (predict_var * odo_var)/(predict_var + odo_var)**2#更新速度方差predicts.append(position_predict)plt.plot(t,predicts,label='kalman filtered position')plt.legend() plt.show()

總結

以上是生活随笔為你收集整理的从理论到实战-如何理解那个把嫦娥送上天的卡尔曼滤波算法Kalman filter?的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。