Kalman Filter (卡尔曼滤波笔记)

数学原理

example

learning opencv3 example 17-01

// Example 17-1. Kalman filter example code
 
#include <vector>
#include <iostream>
#include <cstdlib>
#include <fstream>
#include <opencv2/opencv.hpp>
 
using std::cout;
using std::endl;
 
#define phi2xy(mat) \
    cv::Point(cvRound(img.cols / 2 + img.cols / 3 * cos(mat.at<float>(0))), \
              cvRound(img.rows / 2 - img.cols / 3 * sin(mat.at<float>(0))))
 
 
void help(char** argv ) {
    cout << "\n"
         << "Example 17-1: code for using cv::KalmanFilter\n"
         << argv[0] << "\n\n"
         << "For example:\n"
         << argv[0] <<"\n\n"
         << "Esc to quit\n"
         << endl;
}
 
int main(int argc, char** argv) {
    help(argv);
 
    // Initialize, create Kalman filter object, window, random number
    // generator etc.
    //
    cv::Mat img(500, 500, CV_8UC3);
    cv::KalmanFilter kalman(2, 1, 0);
 
    // state is (phi, delta_phi) - angle and angular velocity
    // Initialize with random guess.
    //
    cv::Mat x_k(2, 1, CV_32F);
    randn(x_k, 0.0, 0.1);
 
    // process noise
    //
    cv::Mat w_k(2, 1, CV_32F);
 
    // measurements, only one parameter for angle
    //
    cv::Mat z_k = cv::Mat::zeros(1, 1, CV_32F);
 
    // Transition matrix 'F' describes relationship between
    // model parameters at step k and at step k+1 (this is
    // the "dynamics" in our model.
    //
    float F[] = {1, 1, 0, 1};
    kalman.transitionMatrix = cv::Mat(2, 2, CV_32F, F).clone();
 
    // Initialize other Kalman filter parameters.
    //
    cv::setIdentity(kalman.measurementMatrix, cv::Scalar(1));
    cv::setIdentity(kalman.processNoiseCov, cv::Scalar(1e-5));
    cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar(1e-1));
    cv::setIdentity(kalman.errorCovPost, cv::Scalar(1));
 
    // choose random initial state
    //
    randn(kalman.statePost, 0.0, 0.1);
 
    for (;;) {
        // predict point position
        //
        cv::Mat y_k = kalman.predict();
 
        // generate measurement (z_k)
        //
        cv::randn(z_k, 0.0,
            sqrt(static_cast<double>(kalman.measurementNoiseCov.at<float>(0, 0))));
        z_k = kalman.measurementMatrix*x_k + z_k;
 
        // plot points (e.g., convert
        //
        img = cv::Scalar::all(0);
        cv::circle(img, phi2xy(z_k), 4, cv::Scalar(128, 255, 255));  // observed
        cv::circle(img, phi2xy(y_k), 4, cv::Scalar(255, 255, 255), 2);  // predicted
        cv::circle(img, phi2xy(x_k), 4, cv::Scalar(0, 0, 255));  // actual to
                                                                 // planar co-ordinates and draw
 
        cv::imshow("Kalman", img);
 
        // adjust Kalman filter state
        //
        kalman.correct(z_k);
 
        // Apply the transition matrix 'F' (e.g., step time forward)
        // and also apply the "process" noise w_k
        //
        cv::randn(w_k, 0.0, sqrt(static_cast<double>(kalman.processNoiseCov.at<float>(0, 0))));
        x_k = kalman.transitionMatrix*x_k + w_k;
 
        // exit if user hits 'Esc'
        if ((cv::waitKey(100) & 255) == 27) {
            break;
        }
    }
 
    return 0;
}
 

参考文献

[1]https://www.bilibili.com/video/BV1MY4y1p7W7?p=7&share_source=copy_web

[2]https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

[3]Adrian Kaehler.learning opencv3.615-634

[4]Welsh, G., and G. Bishop. “An introduction to the Kalman filter” (Techni‐
cal Report TR95-041), University of North Carolina, Chapel Hill, NC, 1995.

[5]aybeck, Peter S. 1979. Stochastic Models, Estimation, and Control, Vol-ume 1, Academic Press, Inc.