欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

Python (kalmanFilter)卡尔曼滤波器

程序员文章站 2024-03-25 22:07:04
...

卡尔曼滤波简介

卡尔曼在利用观测数据估计系统状态时,可以滤除观测时存在的噪声,因此这一过程也被看作是一个滤波过程。

跟踪鼠标

绿色为测量到的鼠标坐标(位置)

红色为卡尔曼滤波器预测的鼠标坐标(位置)

# -*- coding: UTF-8 -*-
# ! /usr/bin/python

import cv2
import numpy as np

# 创建一个大小800*800的空帧
frame = np.zeros((800, 800, 3), np.uint8)
# 初始化测量坐标和鼠标运动预测的数组
last_measurement = current_measurement = np.array((2, 1), np.float32)
last_predicition = current_prediction = np.zeros((2, 1), np.float32)

'''
    mousemove()函数在这里的作用就是传递X,Y的坐标值,便于对轨迹进行卡尔曼滤波
'''
def mousemove(event, x, y, s, p):
    # 定义全局变量
    global frame, current_measurement, last_measurement, current_prediction, last_prediction
    # 初始化
    last_measurement = current_measurement
    last_prediction = current_prediction
    # 传递当前测量坐标值
    current_measurement = np.array([[np.float32(x)], [np.float32(y)]])
    # 用来修正卡尔曼滤波的预测结果
    kalman.correct(current_measurement) # 用当前测量来校正卡尔曼滤波器
    # 调用kalman这个类的predict方法得到状态的预测值矩阵,用来估算目标位置
    current_prediction = kalman.predict()
    # 上一次测量值
    lmx, lmy = last_measurement[0], last_measurement[1]
    # 当前测量值
    cmx, cmy = current_measurement[0], current_measurement[1]
    # 上一次预测值
    lpx, lpy = last_prediction[0], last_prediction[1]
    # 当前预测值
    cpx, cpy = current_prediction[0], current_prediction[1]
    # 绘制测量值轨迹(绿色)
    cv2.line(frame, (lmx, lmy), (cmx, cmy), (0, 100, 0))
    # 绘制预测值轨迹(红色)
    cv2.line(frame, (lpx, lpy), (cpx, cpy), (0, 0, 200))

cv2.namedWindow("kalman_tracker")
# 调用函数处理鼠标事件,具体事件必须由回调函数的第一个参数来处理,该参数确定触发事件的类型(点击和移动)
'''
void setMousecallback(const string& winname, MouseCallback onMouse, void* userdata=0)
       winname:窗口的名字
       onMouse:鼠标响应函数,回调函数。指定窗口里每次鼠标时间发生的时候,被调用的函数指针。
                这个函数的原型应该为void on_Mouse(int event, int x, int y, int flags, void* param);
       userdate:传给回调函数的参数

 void on_Mouse(int event, int x, int y, int flags, void* param);
        event是 CV_EVENT_*变量之一
        x和y是鼠标指针在图像坐标系的坐标(不是窗口坐标系)
        flags是CV_EVENT_FLAG的组合, param是用户定义的传递到setMouseCallback函数调用的参数。
    常用的event:
        CV_EVENT_MOUSEMOVE
        CV_EVENT_LBUTTONDOWN
        CV_EVENT_RBUTTONDOWN
        CV_EVENT_LBUTTONUP
        CV_EVENT_RBUTTONUP
        和标志位flags有关的:
        CV_EVENT_FLAG_LBUTTON
'''
cv2.setMouseCallback("kalman_tracker", mousemove)
'''
Kalman这个类需要初始化下面变量:
转移矩阵,测量矩阵,控制向量(没有的话,就是0),
过程噪声协方差矩阵,测量噪声协方差矩阵,
后验错误协方差矩阵,前一状态校正后的值,当前观察值。
    在此cv2.KalmanFilter(4,2)表示转移矩阵维度为4,测量矩阵维度为2

卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式:
            X(k) = F(k) * X(k-1) + B(k)*U(k) + W(k)
其中
F(k)  是作用在xk−1上的状态变换模型(/矩阵/矢量)。 
B(k)  是作用在控制器向量uk上的输入-控制模型。 
W(k)  是过程噪声,并假定其符合均值为零,协方差矩阵为Qk的多元正态分布。
'''
kalman = cv2.KalmanFilter(4, 2) # 创建kalman滤波器 dynam_params:状态空间的维数;measure_param:测量值的维数;control_params:控制向量的维数,默认为0。由于这里该模型中并没有控制变量,因此也为0。
# 设置测量矩阵
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
# 设置转移矩阵
kalman.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
# 设置过程噪声协方差矩阵
kalman.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) * 0.03

while True:
    cv2.imshow("kalman_tracker", frame)
    if (cv2.waitKey(30) & 0xff) == 27:
        break

cv2.destroyAllWindows()

建立鼠标运动的模型,至少有两个状态变量:鼠标位置x,y,也可以是四个状态变量:位置x,y和速度vx,vy。两个测量变量:鼠标位置x,y。

Python (kalmanFilter)卡尔曼滤波器