我竟然用OpenCV实现了卡尔曼滤波

2021/09/15 10:05
阅读数 708

点击上方小白学视觉”,选择加"星标"或“置顶

重磅干货,第一时间送达

本文转载自:OpenCV学堂(CVSCHOOL)

  
  
  
卡尔曼滤波原理


卡尔曼滤波最早可以追溯到Wiener滤波,不同的是卡尔曼采用状态空间来描述它的滤波器,卡尔曼滤波器同时具有模糊/平滑与预测功能,特别是后者在视频分析与对象跟踪应用场景中被发扬光大,在离散空间(图像或者视频帧)使用卡尔曼滤波器相对简单。假设我们根据一个处理想知道一个变量值如下:

最终卡尔曼滤波完整的评估与空间预测模型工作流程如下:

OpenCV API


cv::KalmanFilter::KalmanFilter(
    int dynamParams, 
    int measureParams,
    int controlParams = 0,
    int type = CV_32F 
)
# dynamParams表示state的维度
# measureParams表示测量维度
# controlParams表示控制向量
# type表示创建的matrices


代码演示


import cv2
from math import cossinsqrt
import numpy as np

if __name__ == "__main__":

    img_height = 500
    img_width = 500
    kalman = cv2.KalmanFilter(210)

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

    while True:
        state = 0.1 * np.random.randn(21)

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

        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[00]
            state_pt = calc_point(state_angle)
            # 预测
            prediction = kalman.predict()
            predict_angle = prediction[00]
            predict_pt = calc_point(predict_angle)

            measurement = kalman.measurementNoiseCov * np.random.randn(11)

            # 生成测量
            measurement = np.dot(kalman.measurementMatrix, state) + measurement
            measurement_angle = measurement[00]
            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, (00255), 3, cv2.LINE_AA, 0)
            cv2.line(img, state_pt, predict_pt, (25500), 3, cv2.LINE_AA, 0)

            # 校正预测与测量值差异
            kalman.correct(measurement)

            # 更新noise矩阵与状态
            process_noise = sqrt(kalman.processNoiseCov[0,0]) * np.random.randn(21)
            state = np.dot(kalman.transitionMatrix, state) + process_noise

            cv2.imshow("Kalman", img)

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

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

    cv2.destroyWindow("Kalman")


好消息,小白学视觉团队的知识星球开通啦,为了感谢大家的支持与厚爱,团队决定将价值149元的知识星球现时免费加入。各位小伙伴们要抓住机会哦!


下载1:OpenCV-Contrib扩展模块中文版教程
在「小白学视觉」公众号后台回复: 扩展模块中文教程 即可下载全网第一份OpenCV扩展模块教程中文版,涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。

下载2:Python视觉实战项目52讲
小白学视觉公众号后台回复:Python视觉实战项目即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目,助力快速学校计算机视觉。

下载3:OpenCV实战项目20讲
小白学视觉公众号后台回复:OpenCV实战项目20讲即可下载含有20个基于OpenCV实现20个实战项目,实现OpenCV学习进阶。

交流群


欢迎加入公众号读者群一起和同行交流,目前有SLAM、三维视觉、传感器自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN算法竞赛等微信群(以后会逐渐细分),请扫描下面微信号加群,备注:”昵称+学校/公司+研究方向“,例如:”张三 + 上海交大 + 视觉SLAM“。请按照格式备注,否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告,否则会请出群,谢谢理解~



本文分享自微信公众号 - 小白学视觉(NoobCV)。
如有侵权,请联系 support@oschina.cn 删除。
本文参与“OSC源创计划”,欢迎正在阅读的你也加入,一起分享。

展开阅读全文
加载中
点击引领话题📣 发布并加入讨论🔥
打赏
0 评论
0 收藏
0
分享
返回顶部
顶部