OpenCV卡爾曼濾波介紹與代碼演示

重磅干貨，第一時間送達

卡爾曼濾波原理

卡爾曼濾波最早可以追溯到Wiener濾波，不同的是卡爾曼采用狀態(tài)空間來描述它的濾波器，卡爾曼濾波器同時具有模糊/平滑與預測功能，特別是后者在視頻分析與對象跟蹤應用場景中被發(fā)揚光大，在離散空間(圖像或者視頻幀)使用卡爾曼濾波器相對簡單。假設我們根據(jù)一個處理想知道一個變量值如下：
最終卡爾曼濾波完整的評估與空間預測模型工作流程如下：
OpenCV API

cv::KalmanFilter::KalmanFilter(
????int?dynamParams,?
????int?measureParams,
????int?controlParams?=?0,
????int?type?=?CV_32F?
)
#?dynamParams表示state的維度
#?measureParams表示測量維度
#?controlParams表示控制向量
#?type表示創(chuàng)建的matrices

代碼演示

import?cv2
from?math?import?cos,?sin,?sqrt
import?numpy?as?np

if?__name__?==?"__main__":

????img_height?=?500
????img_width?=?500
????kalman?=?cv2.KalmanFilter(2,?1,?0)

????cv2.namedWindow("Kalman",?cv2.WINDOW_AUTOSIZE)

????while?True:
????????state?=?0.1?*?np.random.randn(2,?1)

????????#?初始化
????????kalman.transitionMatrix?=?np.array([[1.,?1.],?[0.,?1.]])
????????kalman.measurementMatrix?=?1.?*?np.ones((1,?2))
????????kalman.processNoiseCov?=?1e-5?*?np.eye(2)
????????kalman.measurementNoiseCov?=?1e-1?*?np.ones((1,?1))
????????kalman.errorCovPost?=?1.?*?np.ones((2,?2))
????????kalman.statePost?=?0.1?*?np.random.randn(2,?1)

????????while?True:
????????????def?calc_point(angle):
????????????????return?(np.around(img_width/2?+?img_width/3*cos(angle),?0).astype(int),
????????????????????????np.around(img_height/2?-?img_width/3*sin(angle),?1).astype(int))

????????????state_angle?=?state[0,?0]
????????????state_pt?=?calc_point(state_angle)
????????????#?預測
????????????prediction?=?kalman.predict()
????????????predict_angle?=?prediction[0,?0]
????????????predict_pt?=?calc_point(predict_angle)

????????????measurement?=?kalman.measurementNoiseCov?*?np.random.randn(1,?1)

????????????#?生成測量
????????????measurement?=?np.dot(kalman.measurementMatrix,?state)?+?measurement
????????????measurement_angle?=?measurement[0,?0]
????????????measurement_pt?=?calc_point(measurement_angle)

????????????#?plot?points
????????????def?draw_cross(center,?color,?d):
????????????????cv2.line(img,
?????????????????????????(center[0]?-?d,?center[1]?-?d),?(center[0]?+?d,?center[1]?+?d),
?????????????????????????color,?1,?cv2.LINE_AA,?0)
????????????????cv2.line(img,
?????????????????????????(center[0]?+?d,?center[1]?-?d),?(center[0]?-?d,?center[1]?+?d),
?????????????????????????color,?1,?cv2.LINE_AA,?0)

????????????img?=?np.zeros((img_height,?img_width,?3),?np.uint8)

????????????cv2.line(img,?state_pt,?measurement_pt,?(0,?0,?255),?3,?cv2.LINE_AA,?0)
????????????cv2.line(img,?state_pt,?predict_pt,?(255,?0,?0),?3,?cv2.LINE_AA,?0)

????????????#?校正預測與測量值差異
????????????kalman.correct(measurement)

????????????#?更新noise矩陣與狀態(tài)
????????????process_noise?=?sqrt(kalman.processNoiseCov[0,0])?*?np.random.randn(2,?1)
????????????state?=?np.dot(kalman.transitionMatrix,?state)?+?process_noise

????????????cv2.imshow("Kalman",?img)

????????????code?=?cv2.waitKey(100)
????????????if?code?!=?-1:
????????????????break

????????if?code?in?[27,?ord('q'),?ord('Q')]:
????????????break

????cv2.destroyWindow("Kalman")

交流群

歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動駕駛、計算攝影、檢測、分割、識別、醫(yī)學影像、GAN、算法競賽等微信群（以后會逐漸細分），請掃描下面微信號加群，備注：”昵稱+學校/公司+研究方向“，例如：”張三?+?上海交大?+?視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據(jù)研究方向邀請進入相關微信群。請勿在群內(nèi)發(fā)送廣告，否則會請出群，謝謝理解~