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

重磅干貨，第一時(shí)間送達(dá)

卡爾曼濾波原理

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

cv::KalmanFilter::KalmanFilter(
????int?dynamParams,?
????int?measureParams,
????int?controlParams?=?0,
????int?type?=?CV_32F?
)
#?dynamParams表示state的維度
#?measureParams表示測(cè)量維度
#?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)
????????????#?預(yù)測(cè)
????????????prediction?=?kalman.predict()
????????????predict_angle?=?prediction[0,?0]
????????????predict_pt?=?calc_point(predict_angle)

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

????????????#?生成測(cè)量
????????????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)

????????????#?校正預(yù)測(cè)與測(cè)量值差異
????????????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")

交流群

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