卡尔曼滤波

卡尔曼滤波简介

卡尔曼滤波是一种高效率的递归滤波器（自回归滤波器），它能够从一系列不完全及包含噪声的测量中，估计动态系统的位置。

​ 卡尔曼滤波的一个典型实例是从一组有限的、包含噪声的对物体位置的观察序列（可能有偏差）预测出物体的位置坐标以及速度。

​ 卡尔曼滤波算法的核心是动态调整参数。

Dynamic model（实时系统）

​ 假设有一系列观测值$y_{1},y_{2},...,y_{t-2},y_{t-1},y_{t}$y1​,y2​,...,yt−2​,yt−1​,yt​，组成了一个set（集合），set数据顺序不可以互换，而series可以互换，互换后得到的结果是一样的。

​ Dynamic model（state-space model（状态空间模型））的中心思想是，观测值$y_{t-2},y_{t-1},y_{t}$yt−2​,yt−1​,yt​之间的关系绝对不是相互独立的，是通过对应的隐状态 $x_{t-2}，x_{t-1},x_{t}$xt−2​，xt−1​,xt​联系起来。隐状态之间都有一个概率，这个概率的意义是已知前一个隐状态的值，下一个隐状态的出现的值的概率。当所有隐状态的值都已经得知了，所有的观测都变成相互独立了。

​ 其中的绿色箭头的概率（transition prob）是$P(x_{t}|x_{t-1})$P(xt​∣xt−1​)，表示根据前面的状态$x_{t-1}$xt−1​，现在状态$x_{t}$xt​的概率是多少；红色的箭头的概率（measurement prob）是$P(y_{t}|x_{t})$P(yt​∣xt​)代表，已知对应隐状态的值，观测值的概率。如果要完全的去定义一个Dynamic model，还需要知道一个初始概率（initial prob）$P(x_{1})$P(x1​)，三个概率可以完全描述一个state-space model。

目标跟踪分析

​ 根据质点坐标和速度，系统的状态变量为$x_{t}=(x,y,v_{x},v_{y})^{T}$xt​=(x,y,vx​,vy​)T，这些量是直接观测得到。系统的观测变量为$y_{t}=(\overline{x},\overline{y})$yt​=(x,y​)。

​ 由速度公式，令$\Delta t=1$Δt=1可得到状态转移矩阵为：

​ $A=\left[\begin{matrix} 1,0,1,0\\ ​ 0,1,0,1\\ ​ 0,0,1,0\\ ​ 0,0,0,1\end{matrix}\right]$A=⎣⎢⎢⎡​1,0,1,0​0,1,0,1​0,0,1,0​0,0,0,1​⎦⎥⎥⎤​

​ $H=\left[\begin{matrix} 1,0,0,0\\0,1,0,0\\ \end{matrix}\right]\$H=[1,0,0,00,1,0,0​] 

卡尔曼滤波五大更新方程

有时间更新和状态更新：

时间更新

​ $\overline{x}_{t}=Ax_{t-1}+Bu_{t-1}$xt​=Axt−1​+But−1​ (1)

​ 通过上一时刻的状态最优值和要施加的控制量来预测当前状态。由只是求取平均值，高斯噪声均值为0，所以可以省去$\omega$ω这一项。

$\overline{x}_{t}$xt​：当前时刻的估计值

$A$A：当前时刻的状态转移矩阵

$x_{t-1}$xt−1​：上一时刻的最优估计值

$B$B：当前时刻的控制矩阵

$u_{t}$ut​：当前时刻的控制量

​ $\overline{P}_{t}=AP_{t-1}A^{T}+Q$Pt​=APt−1​AT+Q (2)

​ 除了预测均值之外，还需要预测值的协方差来计算Kalman增益。由上一次的误差协方差矩阵$P_{k-1}$Pk−1​和过程噪声$Q$Q预测新的误差$\overline{P}_{k}$Pk​。

$P_{k-1}$Pk−1​：上一时刻预测值的协方差矩阵

$Q$Q：当前时刻测量值的噪声矩阵

状态更新

​ $K_{t}=\overline{P}_{t}C^{T}(C\overline{P}_{t}C^{T}+R)^{-1}$Kt​=Pt​CT(CPt​CT+R)−1 (3)

​ 根据预测值的协方差$\overline{P}_{t}$Pt​，测量值和状态的比例系数$C$C，测量值的协方差$R$R计算Kalman增益$K_{t}$Kt​。

​ $x_{t}=\overline{x}_{t}+K_{t}(y_{t}-C\overline{x}_{t})$xt​=xt​+Kt​(yt​−Cxt​) (4)

​ 这一步是Kalman Filter的精华部分，现在已经有了对状态的预测值和协方差同时也收集到了状态的测量值，此时可以通过Kalman增益来计算状态估计值，进行校正更新。增益越大，表明越相信测量值。

​ $P_{t}=(I-K_{t}C)\overline{P}_{t}$Pt​=(I−Kt​C)Pt​ (5)

​ 为了进行下一次迭代，需要计算当前状态的协方差。

​ Kalman Filter的算法中，从初始状态开始，不断计算当前状态的均值和方差来迭代，直至系统结束。

​ 卡尔曼滤波有以下两种作用。第一，用作单种数据滤波，那么将数据作为测量量创数模型，卡尔曼滤波通过上一次的值估计出下一次的值，然后将此次的估计值和测量值分别取一定的权重（卡尔曼增益），从而求出这次的最优质。第二，多数据的融合，将一种数据作为测量量，另一种数据作为估计值进行融合。

代码实现——小球跟踪

// Kalman_F.cpp: 定义控制台应用程序的入口点。

#include "stdafx.h"

#include <video/tracking.hpp>
#include <highgui/highgui.hpp>
#include <cmath>  
#include <vector>
#include <stdio.h>
#include <iostream>

using namespace cv;
using namespace std;
const int winHeight = 600;
const int winWidth = 800;

Point move_mouse = Point(winWidth >> 1, winHeight >> 1);		//右移一位，相当于除2
//mouse event callback  

void mouseEvent(int event, int x, int y, int flags, void *param)
{
	if (event == CV_EVENT_MOUSEMOVE)		//滑动鼠标
	{
		move_mouse = Point(x, y);
	}
}


int main()
{

	//1.kalman filter setup
	const int stateNum = 4;		//状态数，（x,y,dx,dy）
	const int measureNum = 2;		//观测量（坐标值）

	KalmanFilter kalman(stateNum, measureNum, 0);
		/* KalmanFilter::KalmanFilter(intdynamParams, intmeasureParams, int controlParams = 0, inttype = CV_32F)
		dynamParams:dimentionality of the state;	measureParams:dimentionality of the measurement
		controlParams:dimentionality of the control vector;	type:Type of the created matrices that should be CV_32F orCV_64F */
	Mat processNoise(stateNum, 1, CV_32F);
	Mat measureNoise(stateNum, 1, CV_32F);
	Mat measurement(measureNum, 1, CV_32F);
	float A[stateNum][stateNum] = {//transition matrix  
		1,0,1,0,
		0,1,0,1,
		0,0,1,0,
		0,0,0,1
	};		//二维就是四个，三维就是六个

	memcpy(kalman.transitionMatrix.data, A, sizeof(A));		//拷贝
	cout << kalman.transitionMatrix << endl;
	//定义矩阵
	//setIdentity: 缩放的单位对角矩阵，给参数矩阵对角线赋相同值
	setIdentity(kalman.measurementMatrix);
	setIdentity(kalman.processNoiseCov, Scalar::all(1e-5));
	setIdentity(kalman.measurementNoiseCov, Scalar::all(1e-1));
	setIdentity(kalman.errorCovPost, Scalar::all(1));

	//initialize post state of kalman filter at random 
	randn(kalman.statePost, Scalar::all(0), Scalar::all(0.1));		//随机生成一个矩阵，期望是0，标准差为0.1;

	namedWindow("kalman", 1);
	setMouseCallback("kalman", mouseEvent);

	Mat img(500, 500, CV_8UC3);
	while (1)
	{
		//2.kalman prediction
		Mat prediction = kalman.predict();
		Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1));

		//3.update measurement 
		measurement.at<float>(0) = (float)move_mouse.x;
		measurement.at<float>(1) = (float)move_mouse.y;

		//4.update
		kalman.correct(measurement);

		//5.draw
		img = Scalar::all(0);
		circle(img, predict_pt, 4, CV_RGB(0, 255, 0), 2);//predicted point with green 
		circle(img, move_mouse, 4, CV_RGB(255, 0, 0), 2);//current position with red
		//定义字体，显示字
		putText(img, "Green: Predicted Point", cvPoint(10, 60), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
		putText(img, "Red: Current Point", cvPoint(10, 90), FONT_HERSHEY_SIMPLEX, 1, Scalar::all(255));
		imshow("kalman", img);
		int key = cvWaitKey(3);
		if (key == 27)
		{
			break;
		}
	}
	system("pause");
	return 0;
}

参考资料：

徐亦达博士关于卡尔曼滤波的公开课，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html

CSDN，再谈卡尔曼滤波–五大公式，https://me.csdn.net/woaizgw)https://blog.csdn.net/woaizgw/article/details/73648578

CSDN，卡尔曼滤波算法的深入理解，https://blog.csdn.net/L_smartworld/article/details/82145013