卡尔曼滤波二维图像点跟踪

原理

参考 https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2

代码

#include <Eigen/Core>
#include <Eigen/Dense>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include <iostream>

/** 
 * predict 
 *  x = Fx + Bu + w   B是作用在控制器向量u上的输入－控制模型
 *  z = Hz + v        其中H是观测模型 v是观测噪声 其均值为零 协方差矩阵为R
 *  p = FpF_trans + Q
 * 
 *  
 * 
 * Update
 * measurement pre-fit residual                 y = z - Hx
 * Innovation (or pre-fit residual) covariance  S = HpH_trans + R
 * Optimal Kalman gain                          K = PHS
 * 
 * 
 * Updated (a posteriori) state estimate        x = x + Ky
 * Updated (a posteriori) estimate covariance   p = (I - KH)P
 * Measurement post-fit residual                y = z - Hx
 * 
 **/


//状态转移矩阵
Eigen::Matrix4d m_F;
//状态矩阵
Eigen::Matrix<double, 4, 1> m_x;
//观测模型
Eigen::Matrix<double, 2, 4> m_H;
//测量矩阵
Eigen::Matrix<double, 2, 1> m_z;
//残差
Eigen::Matrix<double, 2, 1> m_y;
//过程噪声
Eigen::Matrix4d m_Q;
//状态协方差矩阵
Eigen::Matrix4d m_P;
//测量噪声
Eigen::Matrix<double, 2, 2> m_R;
//kalman增益
Eigen::Matrix<double, 4, 2> m_K;
//covariance
Eigen::Matrix2d m_S;

Eigen::Matrix4d m_I{Eigen::Matrix4d::Identity()};

cv::Point mousePos(0, 0);
void mouseMoveCallback(int event, int x, int y, int flags, void* userData) {
  if (event == cv::EVENT_MOUSEMOVE) {
    mousePos.x = x;
    mousePos.y = y;
  }
}
void init(double X, double Y){
  m_F << 1, 0, 1, 0,
         0, 1, 0, 1,
         0, 0, 1, 0,
         0, 0, 0, 1;

  m_x << X, //pose x
         Y, //pose y
         1, //vel x
         1; //vel y

  m_H << 1, 0, 0, 0,
	       0, 1, 0, 0;

  m_Q << 1, 0, 1, 0,
         0, 1, 0, 1,
         0, 0, 1, 0,
         0, 0, 0, 1;

  m_P << 1, 0, 1, 0,
         0, 1, 0, 1,
         0, 0, 1, 0,
         0, 0, 0, 1;

  m_R << 1, 0,
	       0, 1;
}
void predict(){
  m_x = m_F * m_x;
  m_P = m_F * m_P * m_F.transpose() + m_Q;
}
void updata(double X, double Y, double* predict_X, double* predict_Y){
  m_z << X,
         Y;
  m_y = m_z - m_H*m_x;
  m_S = m_H*m_P*m_H.transpose() + m_R;
  m_K = m_P*m_H.transpose()*m_S.inverse();

  m_x = m_x + m_K*m_y;
  m_P = (m_I - m_K*m_H)*m_P;
  *predict_X = m_x[0];
  *predict_Y = m_x[1];
}
int main(){
  cv::namedWindow("kalman2D");
	cv::setMouseCallback("kalman2D", mouseMoveCallback);
  cv::Mat image(500, 500, CV_8UC3);
  init(mousePos.x, mousePos.y);//初始化
  double km_x;
  double km_y;
  while (1) {
    updata(mousePos.x, mousePos.y, &km_x, &km_y);
    predict();
    circle(image, mousePos, 13, cv::Scalar(0, 255, 0), 3); //真实点, 用空心圈表示
    circle(image, cv::Point(km_x, km_y), 9, cv::Scalar(255, 0, 255), -1); //状态向量, 用实心点表示
    cv::imshow("kalman2D", image);
    image = cv::Scalar::all(0);
		int key = cv::waitKey(3);
		if (key == 27) {//esc   
			break;
    }
  }
}

/* int main(void) {
  cv::KalmanFilter kalmanFilter(4, 2, 0); //状态向量四维, 测量变量二维, 无控制向量

  float F[4][4] = { { 1, 0, 1, 0 },
                    { 0, 1, 0, 1 },
                    { 0, 0, 1, 0 },
                    { 0, 0, 0, 1 } };
  kalmanFilter.transitionMatrix = cv::Mat(4, 4, CV_32F, F);

  cv::Mat matMeasurement(2, 1, CV_32F, cv::Scalar::all(0));

  kalmanFilter.statePre.at<float>(0) = (float)mousePos.x;
  kalmanFilter.statePre.at<float>(1) = (float)mousePos.y;
  kalmanFilter.statePre.at<float>(2) = 0;
  kalmanFilter.statePre.at<float>(3) = 0;

  setIdentity(kalmanFilter.measurementMatrix); //H
  setIdentity(kalmanFilter.processNoiseCov, cv::Scalar::all(1e-4)); //Q
  setIdentity(kalmanFilter.measurementNoiseCov, cv::Scalar::all(10)); //R, 设置越小回归越快，但转弯时容易冲出太远
  setIdentity(kalmanFilter.errorCovPost, cv::Scalar::all(.1)); //\sigma_k

  cv::Mat image(500, 500, CV_8UC3);

  std::vector<cv::Point> mousevec, kalmanvec; //这是为了画出轨迹而准备的点的向量组

  mousevec.clear();
  kalmanvec.clear();

  cv::namedWindow("image");
  cv::setMouseCallback("image", mouseMoveCallback);

  while (1) {
    cv::Mat matPrediction = kalmanFilter.predict(); //更新statePre

    //更新测量向量
    matMeasurement.at<float>(0, 0) = (float)mousePos.x;
    matMeasurement.at<float>(1, 0) = (float)mousePos.y;

    cv::Mat estimated = kalmanFilter.correct(matMeasurement); //更新statePost

    cv::Point measPt((int)matMeasurement.at<float>(0, 0), (int)matMeasurement.at<float>(1, 0));
    cv::Point statePt((int)estimated.at<float>(0), (int)estimated.at<float>(1));

    imshow("image", image);
    image = cv::Scalar::all(0);

    mousevec.push_back(measPt);
    kalmanvec.push_back(statePt);

    circle(image, measPt, 13, cv::Scalar(0, 255, 0), 3); //真实点, 用空心圈表示
    circle(image, statePt, 9, cv::Scalar(255, 0, 255), -1); //状态向量, 用实心点表示
  
    if (cv::waitKey(10) == 27)break;
  }

  return 0;
}
 */

cmakelist

cmake_minimum_required(VERSION 3.8)

project(TEST)

find_package(OpenCV 4.7 REQUIRED)
find_package(Eigen3 REQUIRED)

message(STATUS "Opnecv ;ibrary status:")
message(STATUS "> version: ${OpenCV_VERSION}")
message(STATUS "libraries: ${OpenCV_LIBS}")
message(STATUS "> include: ${OpenCV_INCLUDE_DIRS}")

include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(kalman cv_kalman.cpp)
target_link_libraries(kalman 
${OpenCV_LIBS}
Eigen3::Eigen)