卡尔曼滤波学习

什么是卡尔曼滤波？

卡尔曼滤波是一种高效率递归滤波器,即只要获知上一时刻状态的估计值以及当前状态的观测值

就可以计算当前状态的估计值,因此不需要记录观测或者估计历史信息。卡尔曼滤波器与大多数

滤波器不同之处,在于它是一种纯粹的时域滤波器,它不需要在频域设计再转换到时域实现。

 

卡尔曼滤波包括两个阶段：

预测(KF.predict)：

使用上一状态的估计，做出当前状态的估计

更新(KF.correct)：

用当前状态的观测值优化在预测阶段获得的预测值,以次获得一个更精确的新估计值

使用卡尔曼滤波之前要考虑,模型是否满足卡尔曼滤波使用条件

以上个两个是卡尔曼滤波需要遵循的两个方程

第一个方程意味着xk的预估值必须通过线性随机方程,任意的xk都是前一个状态值xk-1和控制信号uk以及过程噪声wk-1的线性组合

下面的方程是观测方程,任意观测值zk(并不确定是否为真值)都是观测信号值xk和测量噪声vk的线性组合。它们都遵循高斯分布

过程噪声和测量噪声相互独立

如果确认了你的模型符合卡尔曼使用条件,就可以开始过程并进行迭代

 

xk-1 表示上个状态的估计值

Pk-1表示上个状态的协方差

 

有兴趣的可以深入研究下面两个链接

http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies

http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

 

这里用OpenCV KalmanFilter举例说明

//构造函数
//@param dynamParams   状态维度
//@param measureParams 测量量维度
//@param controlParams 控制向量维度
//@param type          创建矩阵的类型只能是CV_32F 或者 CV_64F

CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );

//预测函数  预测下个状态值
//@param control 控制矩阵
//@return        预测状态
CV_WRAP const Mat& predict(const Mat& control=Mat());  

//更新  根据观测值优化预测状态值
//@param  measurement 观测状态
//@return             优化后的状态值
CV_WRAP const Mat& correct(const Mat& measurement); 

 

以  y = kx + b  这个函数为例  

 

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <stdio.h>
using namespace std;
using namespace cv;

int main(int, char**)
{
    double k = 10;
    double b = 15;
    
    cout << " origin k = " << k << " " << "b = " << b << endl;
    
    int N = 100;            //数据长度
    double w_sigma = 0.1;
    
    cv::RNG rng;
    
    vector<double>  x_data,y_data;
    
    cout << "generating data: " << endl;
    
    //生成带噪声的数据
    for(int i = 0; i < N; ++i)
    {
        double x = i;
        x_data.emplace_back(x);
        y_data.emplace_back(k * x + b + rng.gaussian(w_sigma));
    }
    
    //声明卡尔曼滤波器
    KalmanFilter KF(2, 1, 0);
    //状态
    Mat state(2, 1, CV_32F);
    //过程噪声
    Mat processNoise(2, 1, CV_32F);
    //参数初始化
    Mat measurement = Mat::zeros(1, 1, CV_32F);
    KF.transitionMatrix = (Mat_<float>(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));
    
    for( int i = 0;i < N; ++i)
    {
        //预估值
        Mat prediction = KF.predict();
        //观测值
        KF.measurementMatrix.at<float>(0,0) = x_data[i];
        KF.measurementMatrix.at<float>(0,1) = 1;
        measurement = y_data[i];
        //获取优化值
        Mat result = KF.correct(measurement);
        cout << "k = " << result.at<float>(0,0) << " "
             << "b = " << result.at<float>(1,0) << endl;
    }
    
    return 0;
}