opencv卡尔曼滤波详解

1.问题描述

假设下面曲线
$$y=kx+b+w$$
其中a,b为曲线的参数，w为高斯噪声。假设我们有N个关于x,y的观测数据点，想根据这些数据点求出直线的参数。

2.卡尔曼滤波拟合点斜式直线

２.1状态变量

状态变量是需要求解的参数。对于拟合一条直线$y=kx+b$，状态变量为待求的参数$k,b$。

2.2状态方程

状态方程可以表示为
$$x_k=x_{k-1}+b+w_k$$

2.3观测方程

虽然点（x,y）是二维的，转换为观测其实只有一维。表示如下
$$h(x_k)=kx+b-y+v_k$$

卡尔曼滤波观测方程中H矩阵如下
$$\begin{gathered} H_k=\frac{\partial h}{\partial x_k}{|}_{{\bar{x}}_k} \\ {H}_k=\left[\begin{array}{cc}\frac{\partial h}{\partial k}& \frac{\partial h}{\partial k}\end{array}\right]=\left[\begin{array}{cc}x& 1\end{array}\right] \end{gathered}$$
设第k次观测的坐标值为$(x_0,y_0)$
将观测值代入得到
第k次观测的$H_k$矩阵为
$$H_k=\left[\begin{array}{cc}{x}_0& 1\end{array}\right]$$

3使用opencv库实现

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp"

#include 

using namespace cv;
using namespace std;
int main(int, char**)
{
    double theta=-M_PI/4;
    double c=10;
    double k=-tan(theta);
    double b=-c/sin(theta);
    cout<<"ori theta ="<(2, 2) << 1, 0, 0, 1);

    setIdentity(KF.measurementMatrix);
    setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
    setIdentity(KF.errorCovPost, Scalar::all(1));

    randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
    for ( int i=0; i(0,0)=x_data[i];
        KF.measurementMatrix.at(0,1)=1;
        measurement=y_data[i];
        KF.correct(measurement);
         cout<<" k= "<(0,0)<<" b= "<(1,0)<

注意：噪声一定不能设置得太大，否则不会收敛，得不得想要的结果。ps：被坑惨了．．．

4.opencv卡尔曼滤波实现详解

4.1 opencv卡尔曼滤波实现源码

namespace cv
{

KalmanFilter::KalmanFilter() {}
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
{
    init(dynamParams, measureParams, controlParams, type);
}

void KalmanFilter::init(int DP, int MP, int CP, int type)
{
    CV_Assert( DP > 0 && MP > 0 );
    CV_Assert( type == CV_32F || type == CV_64F );
    CP = std::max(CP, 0);

    statePre = Mat::zeros(DP, 1, type);
    statePost = Mat::zeros(DP, 1, type);
    transitionMatrix = Mat::eye(DP, DP, type);

    processNoiseCov = Mat::eye(DP, DP, type);
    measurementMatrix = Mat::zeros(MP, DP, type);
    measurementNoiseCov = Mat::eye(MP, MP, type);

    errorCovPre = Mat::zeros(DP, DP, type);
    errorCovPost = Mat::zeros(DP, DP, type);
    gain = Mat::zeros(DP, MP, type);

    if( CP > 0 )
        controlMatrix = Mat::zeros(DP, CP, type);
    else
        controlMatrix.release();

    temp1.create(DP, DP, type);
    temp2.create(MP, DP, type);
    temp3.create(MP, MP, type);
    temp4.create(MP, DP, type);
    temp5.create(MP, 1, type);
}

const Mat& KalmanFilter::predict(const Mat& control)
{
    CV_INSTRUMENT_REGION()

    // update the state: x'(k) = A*x(k)
    statePre = transitionMatrix*statePost;

    if( !control.empty() )
        // x'(k) = x'(k) + B*u(k)
        statePre += controlMatrix*control;

    // update error covariance matrices: temp1 = A*P(k)
    temp1 = transitionMatrix*errorCovPost;

    // P'(k) = temp1*At + Q
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);

    // handle the case when there will be measurement before the next predict.
    statePre.copyTo(statePost);
    errorCovPre.copyTo(errorCovPost);

    return statePre;
}

const Mat& KalmanFilter::correct(const Mat& measurement)
{
    CV_INSTRUMENT_REGION()

    // temp2 = H*P'(k)
    temp2 = measurementMatrix * errorCovPre;

    // temp3 = temp2*Ht + R
    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);

    // temp4 = inv(temp3)*temp2 = Kt(k)
    solve(temp3, temp2, temp4, DECOMP_SVD);

    // K(k)
    gain = temp4.t();

    // temp5 = z(k) - H*x'(k)
    temp5 = measurement - measurementMatrix*statePre;

    // x(k) = x'(k) + K(k)*temp5
    statePost = statePre + gain*temp5;

    // P(k) = P'(k) - K(k)*temp2
    errorCovPost = errorCovPre - gain*temp2;

    return statePost;
}

}

以下为公式对应源码部分

4.2预测

预测对应predict函数
状态方程更新
$${\bar{x}}_k=F_k\hat{x}+u_k$$
对应

// update the state: x'(k) = A*x(k)
statePre = transitionMatrix*statePost;
 if( !control.empty() )
 // x'(k) = x'(k) + B*u(k)
 　　statePre += controlMatrix*control;

Ｐ更新
$${\bar{P}}_k=F{\hat{P}}_k{F}^T+R_k$$
对应

  // update error covariance matrices: temp1 = A*P(k)
    temp1 = transitionMatrix*errorCovPost;
    // P'(k) = temp1*At + Q
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);

４.３更新

（１）先计算卡尔曼增益$K$
$$K={\bar{P}}_k{H}_k^T(H_k{\bar{P}}_k{H}_k^T+Q{)}^{-1}$$
对应

  // temp2 = H*P'(k)
 temp2 = measurementMatrix * errorCovPre;
// temp3 = temp2*Ht + R
gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
// temp4 = inv(temp3)*temp2 = Kt(k)
solve(temp3, temp2, temp4, DECOMP_SVD);
// K(k)
gain = temp4.t();

其中H矩阵是对应measurementMatrix是需要手动输入的
$$H=\frac{\partial h}{\partial x_k}{|}_{{\bar{x}}_k}$$

（２）计算后验分布
计算增量
$${\hat{x}}_k={\bar{x}}_k+K(z_k-H_k{\bar{x}}_k)$$
对应

// temp5 = z(k) - H*x'(k)
    temp5 = measurement - measurementMatrix*statePre;
 // x(k) = x'(k) + K(k)*temp5
    statePost = statePre + gain*temp5;

更新P后验
$${\hat{P}}_k=(I-K{H}_k){\bar{P}}_k$$
对应

// P(k) = P'(k) - K(k)*temp2
    errorCovPost = errorCovPre - gain*temp2;

５Eigen实现

这是我自己按照公式实现的代码，对应关系更加明显

#include "ceres/ceres.h"
#include 
#include
using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;
const int STATE_NUMBER=2;
const int MEASUREMENT_NUMBER=1;
const double SIGMA=0.01;
int main(int argc, char** argv)
{
  double k=1.0, b=2.0;         // 真实参数值
  int N=100;                          // 数据点
  double w_sigma=1;                 // 噪声Sigma值
  cv::RNG rng;                        // OpenCV随机数产生器
  vector x_data, y_data;      // 数据
  cout<<"generating data: "<