卡尔曼滤波

@(机器学习)[状态方程|观测方程]

• 卡尔曼滤波

卡尔曼滤波简介

如果你要测量的东西很容易测准确，没有什么随机干扰，那就不需要劳驾卡尔曼。有的时候，我们的测量因为随机干扰，无法准确得到，卡尔曼就给我们想了个办法，让我们在干扰为高斯分布的情况下，得到的测量均方误差最小，也就是测量值扰动最小，看起来最平滑1。

卡尔曼滤波基本公式

• 滤波估计方程（K时刻的最优值）：
  

  xk=xk/k−1+Bk−1uk−1+Jk−1Zk−1+Hk[Zk−CK∗xk/k−1−Ck∗Bk−1uk−1]
  • xk为k时刻最优值，
  • xk/k−1为根据k−1时刻进行预测得到的k时刻的预测值，由状态一步预测方程得到；
  • uk−1是控制量，Bk−1为k−1时刻控制系数矩阵
  • Jk−1Zk−1分别是过程激励噪声，可以简单理解为噪声

• 状态一步预测方程
  

  xk/k−1=Ak,k−1xk−1+Bk−1uk−1+Jk−1Zk−1
  • uk−1是控制量

• 均方误差一步预测：
  

  Pk/k−1=Ak,k−1∗Pk−1∗ATk,k−1+Tk−1Qk−1TTk−1−Jk−1STk−1TTk−1
  • Tk−1Qk−1TTk−1−Jk−1STk−1TTk−1为观测噪声，
  • Qk为过程噪声的协方差，其为非负定矩阵；

• 均方误差更新矩阵（K时刻最优均方误差）：
  

  Pk=[I−Hk∗Ck]∗Pk/k−1

• 滤波增益方程（权重系数更新）：
  

  Hk=Pk/k−1∗CTk[Ck∗Pk/k−1∗CTk+Rk]−1
  
  其中，Rk为测量噪声的协方差，其为正定矩阵；

opencv源码剖析

头文件中类声明如下：

class CV_EXPORTS_W KalmanFilter
{
public:
    CV_WRAP KalmanFilter();
    /** @overload
    @param dynamParams Dimensionality of the state.
    @param measureParams Dimensionality of the measurement.
    @param controlParams Dimensionality of the control vector.
    @param type Type of the created matrices that should be CV_32F or CV_64F.
    */
    CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );

    /** @brief Re-initializes Kalman filter. The previous content is destroyed.
    @param dynamParams Dimensionality of the state.
    @param measureParams Dimensionality of the measurement.
    @param controlParams Dimensionality of the control vector.
    @param type Type of the created matrices that should be CV_32F or CV_64F.
     */
    void init( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );

    /** @brief Computes a predicted state.
    @param control The optional input control
     */
    CV_WRAP const Mat& predict( const Mat& control = Mat() );

    /** @brief Updates the predicted state from the measurement.
    @param measurement The measured system parameters
     */
    CV_WRAP const Mat& correct( const Mat& measurement );

    CV_PROP_RW Mat statePre;           //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
    CV_PROP_RW Mat statePost;          //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
    CV_PROP_RW Mat transitionMatrix;   //!< state transition matrix (A)
    CV_PROP_RW Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
    CV_PROP_RW Mat measurementMatrix;  //!< measurement matrix (H)
    CV_PROP_RW Mat processNoiseCov;    //!< process noise covariance matrix (Q)
    CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
    CV_PROP_RW Mat errorCovPre;        //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
    CV_PROP_RW Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
    CV_PROP_RW Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

    // temporary matrices
    Mat temp1;
    Mat temp2;
    Mat temp3;
    Mat temp4;
    Mat temp5;
};

源文件中类定义如下：

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;
}

}

算法应用实例-目标跟踪

static inline Point calcPoint(Point2f center, double R, double angle)
{
    return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}

// plot points
static inline void drawCross(cv::Mat& img, Point& center, Color color, float d ) 
{
    cv::line(img, Point( center.x - d, center.y - d ),Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0);
    cv::line(img, Point( center.x + d, center.y - d ),Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 );
}


int kalmanTracking()
{
    Mat img(500, 500, CV_8UC3);
    /*KalmanFilter (int dynamParams, int measureParams, int controlParams=0, int type=CV_32F)
     *dynamParams : 状态转移参数
     *measureParams : 测量参数
     *controlParams : 运动控制参数
     */
    KalmanFilter KF(2, 1, 0);  //dynamParams:2    measureParams:1
    Mat state(2, 1, CV_32F); /* (phi, delta_phi) */
    Mat processNoise(2, 1, CV_32F);
    Mat measurement = Mat::zeros(1, 1, CV_32F);  //测量值
    char code = (char)-1;

    //初始化状态
    randn(state, Scalar::all(0), Scalar::all(0.1));
    //初始化kalman参数
    KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 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 (;;)
        {
            Point2f center(img.cols*0.5f, img.rows*0.5f);
            float R = img.cols / 3.f;
            double stateAngle = state.at<float>(0);
            Point statePt = calcPoint(center, R, stateAngle);

            //predict angle
            Mat prediction = KF.predict();  //Computes a predicted state.
            double predictAngle = prediction.at<float>(0);
            Point predictPt = calcPoint(center, R, predictAngle);

            randn(measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0)));

            // generate measurement
            measurement += KF.measurementMatrix*state;

            double measAngle = measurement.at<float>(0);
            Point measPt = calcPoint(center, R, measAngle);



            img = Scalar::all(0);
            drawCross(statePt, Scalar(255, 255, 255), 3);
            drawCross(measPt, Scalar(0, 0, 255), 3);
            drawCross(predictPt, Scalar(0, 255, 0), 3);
            line(img, statePt, measPt, Scalar(0, 0, 255), 3, LINE_AA, 0);
            line(img, statePt, predictPt, Scalar(0, 255, 255), 3, LINE_AA, 0);

            if (theRNG().uniform(0, 4) != 0)
                KF.correct(measurement);

            randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
            state = KF.transitionMatrix*state + processNoise;

            imshow("Kalman", img);
            code = (char)waitKey(100);

            if (code > 0)
                break;
        }
        //if (code == 27 || code == 'q' || code == 'Q')
            //break;
    //}

    return 0;
}

---

---

1. http://blog.csdn.net/MangZuo/article/details/71171137 ↩