卡尔曼滤波，个人理解类似于一种信息融合算法，它是将预测值和测量值进行融合，得到一个估计值。因此需要一个预测（状态）方程和一个观测方程。即：
基本模型
这里用一个小车在一维坐标系中做匀加速直线运动来说明参数含义。预测方程：
预测方程是利用线性差分方程对下一状态进行估计。例如匀速运动问题，室温问题等。其方程表达式如下：
参数含义：假设一个小车，其初始状态为
，其加速度为
，那么可以得到如下方程：
转换为矩阵形式如下：
测量方程
测量方程是由当前状态估计值转换为测量值得一种线性函数，该过程获取的到不是实际传感器测量的结果，可以理解为是一种预测测量值。可表示为：
其中预测噪声w和测量噪声v是相互独立的，且是高斯白噪声，服从高斯分布：
推到过程忽略，直接到结论
实例
下面是利用opencv中的卡尔曼滤波函数实现鼠标跟踪，其中绿点为每次测量值，黄点为估计值。
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include<iostream>
using namespace cv;
using namespace std;
const int winHeight = 600;
const int winWidth = 800;
Point mousePosition = Point(winWidth >> 1, winHeight >> 1);
//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param)
{
if (event == CV_EVENT_MOUSEMOVE)
{
mousePosition = Point(x, y);
}
}
int main(void)
{
Mat processNoise(2, 1, CV_32F);//噪声
RNG rng;
//1.kalman filter setup
const int stateNum = 4; //状态值4×1向量(x,y,△x,△y)
const int measureNum = 2; //测量值2×1向量(x,y)
KalmanFilter KF(stateNum, measureNum, 0);
KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); //转移矩阵A
setIdentity(KF.measurementMatrix); //测量矩阵H
setIdentity(KF.processNoiseCov, Scalar::all(5)); //系统噪声方差矩阵Q
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1)); //测量噪声方差矩阵R
setIdentity(KF.errorCovPost, Scalar::all(1)); //后验错误估计协方差矩阵P
rng.fill(KF.statePost, RNG::UNIFORM, 0, winHeight>winWidth ? winWidth : winHeight); //初始状态值x(0)
Mat measurement = Mat::zeros(measureNum, 1, CV_32F); //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义
// 初始化值
cout << "Time Update:" << endl;
cout << "X(k)_ = A*X(k-1)_" << endl;
cout << "P(k)_ = A*P(k-1)*AT + Q" << endl << endl;
cout << "Measurement Update:" << endl;
cout << "K(k) = P(k)_ * HT * inv(H * P(k)_ * HT + R)" << endl; // K(k)为卡尔曼增益
cout << "X(k) = X(k)_ + K(k) * (Z(k) - H * X(k)_)" << endl;
cout << "P(k) = (I - K(k) * H) * P(k)_" << endl << endl;
cout << "初始估计值X: " << endl << KF.statePost << endl;
cout << "转移矩阵A: " << endl << KF.transitionMatrix << endl;
cout << "后验错误估计协方差矩阵P: " << endl << KF.errorCovPost << endl;
cout << "系统噪声方差矩阵Q: " << endl << KF.processNoiseCov << endl << endl;
cout << "测量值Z: " << endl << measurement << endl;
cout << "测量矩阵H: " << endl << KF.measurementMatrix << endl;
cout << "测量噪声方差矩阵R: " << endl << KF.measurementNoiseCov << endl;
namedWindow("kalman");
setMouseCallback("kalman", mouseEvent);
Mat image(winHeight, winWidth, CV_8UC3, Scalar(0));
while (1)
{
//2.kalman prediction
Mat prediction = KF.predict();
Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1)); //预测值(x',y')
//3.update measurement
randn(processNoise, Scalar::all(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));//产生随机噪声，即模拟测量噪声
measurement.at<float>(0) = (float)(mousePosition.x + processNoise.at<float>(0,0));
measurement.at<float>(1) = (float)(mousePosition.y + processNoise.at<float>(1,0));
Point p;
p.x = measurement.at<float>(0);
p.y = measurement.at<float>(1);
//4.update
KF.correct(measurement);
//draw
image.setTo(Scalar(255, 255, 255, 0));
circle(image, predict_pt, 5, Scalar(0, 255, 0), 3); //predicted point with green
circle(image, mousePosition, 5, Scalar(255, 0, 0), 3); //current position with red
putText(image, "predicted position:" + to_string(predict_pt.x) + "," + to_string(predict_pt.y), Point(10, 30), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);
putText(image, " current position:" + to_string(mousePosition.x) + "," + to_string(mousePosition.y), cvPoint(10, 60), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);
imshow("kalman", image);
int key = waitKey(100);
if (key == 27)
{
break;
}
}
}结果：
