kalman跟踪的实现
卡尔曼滤波应用广泛且功能强大，它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。
其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做
出满足最小均方误差的估计。
卡尔曼滤波器包括两个主要过程：预估与校正。
预估过程
主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；
校正过程
负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计。
这样的一个过程，我们称之为预估－校正过程，对应的这种估计算法称为预估－校正算法。
卡尔曼滤波器的递归过程：
1) 估计时刻k 的状态：
X(k) = A*X(k-1) + B*u(k)
这里, u(k) 是系统输入
2) 计算误差相关矩阵P, 度量估计值的精确程度：
P(k) = A*P(k-1)*A’+ Q
这里, Q = E{ Wj^2 } 是系统噪声的协方差阵，即系统框图中的Wj的协方差阵, Q 应该是不断变化的，为了简化，当作一个常数矩阵。
3) 计算卡尔曼增益, 以下略去 (k), 即 P = P(k), X = X(k)：
K = P *C’ * (C * P * C’ + R) -1
这里 R = E{ Vj^2 }, 是测量噪声的协方差(阵), 即系统框图中的 Vj 的协方差, 为了简化，也当作一个常数矩阵。由于我们的系统一般是单输入单输出，所以 R是一个 1x1的矩阵，即一个常数，上面的公式可以简化为:
K = P *C’ / (C * P * C’ + R)
4) 状态变量反馈的误差量：
e = Z(k) – C*X(k)
这里的 Z(k) 是带噪声的测量量
5) 更新误差相关矩阵P
P = P – K * C * P
6) 更新状态变量：
X =X + K*e = X + K* (Z(k) – C*X(k))
7) 最后的输出：
Y = C*X
基本思想
利用观测数据对状态变量的预测估计进行修正，以得到状态变量的最优估计，即
最优估计＝预测估计＋修正
（1）算法是递推的，时域内设计滤波器，适用于多维随机过程的估计；
（2）用递推法计算，不需要知道全部过去的值。用状态方程描述状态变量的动态变化规律，因此，信号可以是平稳的，也可以是非平稳的；
（3）误差准则仍为均方误差最小准则。
首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出。
首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。
到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表covariance：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
P(k|k)是后验估计误差协方差矩阵，度量估计值的精确程度
其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。
式子1，2，3，4和5就是卡尔曼滤波器的5个基本公式。
kalman滤波器工作原理用图表示如下
kalman滤波器用于跟踪时用之前观测到的值预测下一步的状态，然后根据观测值对自己作出修正，如此不断迭代，以实现跟踪。
你可能会想，kalman滤波器到底有什么用呢，既然我每次都能观测到测量的状态值，比如速度，那我还要kalman滤波器来预测速度干嘛，而且它得到的的预测值还不一定准？我的理解是你之所以能够观测到状态值，完全是因为kalman滤波器的功劳。假设一个装有kalman滤波器的追踪器（tracker）在跟踪一个目标（target），tracker不知道target下一时刻的速度（包括大小和方向），tracker有的只是前一时刻它观测到的target的速度，现在tracker要决定下一时刻自己要往哪个方向运动，运动多快，一旦决策失误，tracker就会丢失target。那该怎么办呢？这就是kalman滤波器发挥作用的时候了，kalman滤波器根据之前的观测值告诉tracker：往12点钟方向走，速度80迈（当然也可能在三维空间中）。于是tracker按照kalman滤波器告诉它的信息执行跟踪，发现成功跟上target，于是tracker把此时它观测到的target状态反馈给kalman滤波器，以方便kalman滤波器做下一步决策。如果没有kalman滤波器，tracker胡乱走一个方向，很可能会跟踪失败。如果聪明一点的话，tracker按照之前它观测到的target的速度来前进，假设这一刻target突然转向，如果没有kalman滤波器，tracker就会离target越来越远，最终跟丢。有kalman滤波器的话，kalman滤波器会告诉tracker说，嘿，老兄，我们该转方向了。你可能会问target转向特别迅速怎么办？那就提高kalman滤波器预测和校正的频率。这样一想，其实kalman滤波器的思想很朴素。
模拟电路中稳压电源也是这种思想。
以上是我个人的理解，如有错误请大家指出。
#ifndef _KALMANFILTER_
#define _KALMANFILTER_
class CKalman
{
public:CKalman(int D, int M, int C);~CKalman();void KalmanPredict(double * control);void KalmanCorrect(double * measurement);void set_transition_matrix(const double*transition_matrix);
public:int MP; //number of measure vector dimensions int DP; //number of state vector dimensions int CP; //number of control vector dimensions double* state_pre; //[DP * 1] double* state_post; //[DP * 1] double* transition_matrix; //[DP * DP] double* control_matrix; //[DP * CP] if (CP > 0) double* measurement_matrix; //[MP * DP] double* process_noise_cov; //[DP * DP] double* measurement_noise_cov;//[MP * MP] double* error_cov_pre; //[DP * DP] double* error_cov_post; //[DP * DP] double* gain; //[DP * MP]
};#endif#include "stdafx.h"
#include "kalmanfilter.h"
#include "Matrix.h"
#include<memory>
#include<assert.h>
using namespace std;CKalman::CKalman(int D, int M, int C){if (D <= 0 || M <= 0){assert(false);// "state and measurement vectors must have positive number of dimensions! ");}if (C < 0){assert(false);// "No Control!");}DP = D;MP = M;CP = C;state_pre = new double[DP * 1];memset(state_pre, 0, sizeof(double)*DP);state_post = new double[DP * 1];memset(state_post, 0, sizeof(*state_post));transition_matrix = new double[DP * DP];memset(transition_matrix, 0, sizeof(double)*DP*DP);process_noise_cov = new double[DP * DP];memset(process_noise_cov, 0, sizeof(double)*DP*DP);measurement_matrix = new double[MP * DP];memset(measurement_matrix, 0, sizeof(double)*MP*DP);measurement_noise_cov = new double[MP * MP];memset(measurement_noise_cov, 0, sizeof(double)*MP*MP);error_cov_pre = new double[DP * DP];memset(error_cov_pre, 0, sizeof(double)*DP*DP);error_cov_post = new double[DP * DP];memset(error_cov_post, 0, sizeof(double)*DP*DP);gain = new double[DP * MP];memset(gain, 0, sizeof(double)*DP*MP);if (CP > 0){control_matrix = new double[DP * CP];memset(control_matrix, 0, sizeof(double)*DP*CP);}
}void CKalman::set_transition_matrix(const double*trans_matrix)
{for (int i = 0; i < DP*DP; i++){transition_matrix[i] = trans_matrix[i];}
}CKalman::~CKalman(){delete[] state_pre;delete[] state_post;delete[] transition_matrix;if (CP >0){delete[] control_matrix;}delete[] measurement_matrix;delete[] process_noise_cov;delete[] measurement_noise_cov;delete[] error_cov_pre;delete[] gain;delete[] error_cov_post;
}void CKalman::KalmanPredict(double * control){CMatrix matrix;/* update the state *//* x'(k) = A*x(k) */matrix.Mul(transition_matrix, DP, DP, state_post, DP, 1, state_pre);if (control && CP > 0){/* x'(k) = x'(k) + B*u(k) */double * temp1 = new double[DP * 1];matrix.Mul(control_matrix, DP, CP, control, CP, 1, temp1);matrix.Add(state_pre, temp1, state_pre, DP, 1);delete[] temp1;temp1 = NULL;}/* update error covariance matrices *//* temp2 = A*P(k) */double * temp2 = new double[DP * DP];matrix.Mul(transition_matrix, DP, DP, error_cov_post, DP, DP, temp2);/* P'(k) = temp2*At + Q */double * temp3 = new double[DP * DP];double * temp4 = new double[DP * DP];matrix.Transpose(transition_matrix, DP, DP, temp3);matrix.Mul(temp2, DP, DP, temp3, DP, DP, temp4);matrix.Add(temp4, process_noise_cov, error_cov_pre, DP, DP);delete[] temp2; temp2 = NULL;delete[] temp3; temp3 = NULL;delete[] temp4; temp4 = NULL;
}void CKalman::KalmanCorrect(double * measurement){if (!measurement){assert(false);// , "Measurement is Null!!!");}CMatrix matrix;/* temp1 = Ht*/double * temp1 = new double[DP * MP];matrix.Transpose(measurement_matrix, MP, DP, temp1);/* temp2 = P'(k) * temp1 */double * temp2 = new double[DP * MP];matrix.Mul(error_cov_pre, DP, DP, temp1, DP, MP, temp2);/* temp3 = H*temp2 + R */double * temp3 = new double[MP * MP];matrix.Mul(measurement_matrix, MP, DP, temp2, DP, MP, temp3);matrix.Add(temp3, measurement_noise_cov, temp3, MP, MP);/* temp4 = inv(temp3) */double * temp4 = new double[MP * MP];matrix.ContraryMatrix(temp3, temp4, MP);/* K(k) = temp2 * temp4 */matrix.Mul(temp2, DP, MP, temp4, MP, MP, gain);delete[] temp1; temp1 = NULL;delete[] temp2; temp2 = NULL;delete[] temp3; temp3 = NULL;delete[] temp4; temp4 = NULL;//update state_post /* temp5 = z(k) - H*x'(k) */double * temp5 = new double[MP * 1];matrix.Mul(measurement_matrix, MP, DP, state_pre, DP, 1, temp5);matrix.Sub(measurement, temp5, temp5, MP, 1);/* x(k) = x'(k) + K(k)*temp5 */double * temp6 = new double[DP * 1];matrix.Mul(gain, DP, MP, temp5, MP, 1, temp6);matrix.Add(state_pre, temp6, state_post, DP, 1);delete[] temp5; temp5 = NULL;delete[] temp6; temp6 = NULL;//update error_cov_post /* P(k) = P'(k) - K(k)*H* P'(k) */double * temp7 = new double[MP * DP];double * temp8 = new double[DP * DP];matrix.Mul(measurement_matrix, MP, DP, error_cov_pre, DP, DP, temp7);matrix.Mul(gain, DP, MP, temp7, MP, DP, temp8);matrix.Sub(error_cov_pre, temp8, error_cov_post, DP, DP);delete[] temp7; temp7 = NULL;delete[] temp8; temp8 = NULL;
}
调用
// kalman-filter.cpp : 定义控制台应用程序的入口点。
//#include "stdafx.h"
#include"kalmanfilter.h"
#include<time.h>
#include<cstdlib>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h> using namespace std;const int winHeight = 600;
const int winWidth = 800;CvPoint mousePosition = cvPoint(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 = cvPoint(x, y);}
}int _tmain(int argc, _TCHAR* argv[])
{time_t t;srand(time(&t));//1.kalman filter setup const int stateNum = 4;const int measureNum = 2;CKalman kf(stateNum, measureNum, 0);//state(x,y,detaX,detaY) double A[] = {//transition matrix 1, 0, 1, 0,0, 1, 0, 1,0, 0, 1, 0,0, 0, 0, 1};kf.set_transition_matrix(A);kf.measurement_matrix[0] = 1;//for x,x is an observationkf.measurement_matrix[5] = 1;//for y,y is an observationkf.measurement_noise_cov[0] = 1e1;kf.measurement_noise_cov[3] = 1e1;kf.process_noise_cov[0] = 1e-1;//diagkf.process_noise_cov[5] = 1e-1;kf.process_noise_cov[10] = 1e-1;kf.process_noise_cov[15] = 1e-1;int nn = winHeight > winWidth ? winWidth : winHeight;kf.state_post[0] = double(rand()) / RAND_MAX*nn;//xkf.state_post[1] = double(rand()) / RAND_MAX*nn;//y kf.state_post[2]=rand();//deltaXkf.state_post[3]=rand();//deltaYkf.error_cov_post[0] = 1;//diagkf.error_cov_post[5] = 1;kf.error_cov_post[10] = 1;kf.error_cov_post[15] = 1;CvFont font;cvInitFont(&font, CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, 1);cvNamedWindow("kalman");cvSetMouseCallback("kalman", mouseEvent);IplImage* img = cvCreateImage(cvSize(winWidth, winHeight), 8, 3);while (1){//2.kalman prediction kf.KalmanPredict(NULL);CvPoint predict_pt = cvPoint((int)kf.state_pre[0], (int)kf.state_pre[1]);//3.update measurement double measure[2];measure[0] = double(mousePosition.x);measure[1] = double(mousePosition.y);//4.update kf.KalmanCorrect(measure);//draw cvSet(img, cvScalar(255, 255, 255, 0));cvCircle(img, predict_pt, 5, CV_RGB(0, 255, 0), 3);//predicted point with green cvCircle(img, mousePosition, 5, CV_RGB(255, 0, 0), 3);//current position with red char buf[256];sprintf_s(buf, 256, "predicted position:(%3d,%3d)", predict_pt.x, predict_pt.y);cvPutText(img, buf, cvPoint(10, 30), &font, CV_RGB(0, 0, 0));sprintf_s(buf, 256, "current position :(%3d,%3d)", mousePosition.x, mousePosition.y);cvPutText(img, buf, cvPoint(10, 60), &font, CV_RGB(0, 0, 0));cvShowImage("kalman", img);int key = cvWaitKey(3);if (key == 27){//esc break;}}cvReleaseImage(&img);return 0;}
为什么卡尔曼滤波只能用于解决线性高斯系统
Kalman Filter(卡尔曼滤波)的推导
中，介绍了卡尔曼滤波几个公式的证明，后验概率密度要是高斯型的这一假设用在了这个计算结果的证明过程里了。同时说明了卡尔曼滤波的实际目的：
1）预测结果是真值的无偏估计
2）各个变量的方差最小
在这篇证明中，作者提到了 http://www.aticourses.com/kalman_filter.pdf 这篇推导，并指出证明的内容主要来源于此。具体内容请读者自己翻阅。
今天小结一下卡尔曼滤波
卡尔曼滤波 -- 从推导到应用
Kalman滤波器从原理到实现
对Kalman(卡尔曼)滤波器的理解
kalman跟踪的实现相关推荐
- python + opencv: kalman 跟踪
之前博文中讲解过kalman滤波的原理和 应用,这里用一个跟踪鼠标的例程来演示怎么在opencv里用自带的kalman函数进行目标跟踪,文章的内容对做图像跟踪有借鉴意义.文章主要是网络资源进行整理和简 ...
- matlab kalman 跟踪,Kalman matlab中基于卡尔曼滤波的目标跟踪程序 272万源代码下载- www.pudn.com...
文件名称: Kalman下载 收藏√ [ 5 4 3 2 1 ] 开发工具: Others 文件大小: 309 KB 上传时间: 2015-05-21 下载次数: 32 提 供 者: he ...
- Control~Kalman filter
常用控制算法(包括PID和卡尔曼滤波等)各有什么天然的局限乃至缺陷 回答都专业而又接地气 自平衡小车的探讨:卡尔曼滤波与PID算法 ...
- Kalman实际应用总结
文章目录 友情提示 Kalman理论介绍 一. 简单理论介绍理论 二. 升华理论介绍 Kalman基本应用 一. Kalman跟踪/滤波 二. Kalman预测/融合(单传感器) 三. Kalman多 ...
- m基于MATLAB-GUI的GPS数据经纬度高度解析与kalman分析软件设计
目录 1.算法概述 2.仿真效果预览 3.MATLAB部分代码预览 4.完整MATLAB程序 1.算法概述 经度纬度和高度来自GPS信号的中的GPGGA的数据.所以提取这三个信息主要是对GPGGA中的 ...
- 实现你的kalman
本文结合opencv的Mat数据类型,实现了kalman跟踪,具体理论知识可以参考博客:http://blog.csdn.net/baimafujinji/article/details/506468 ...
- 计算机论文写作提纲怎么写,计算机算法论文提纲 计算机算法论文大纲如何写...
为论文写作提供[100个]计算机算法论文提纲,海量计算机算法相关论文提纲,包括专科与本科以及硕士论文提纲,解决您的计算机算法论文大纲如何写的相关难题! 五.一种求解Ramsey数的计算机算法论文提纲 ...
- DSS代码解读-SRR_DSS_initTask(十二)
摘要:该线程函数是DSS初始化线程任务,初始化DSS子系统中的各个组件.接下来将对这个线程函数进行逐行注释讲解.代码是按照工程顺序讲解的. 1.函数参数 通过ctrl+右键跳转进入到该线程函数,首先可 ...
- OpenCV学习笔记（三十六）——Kalman滤波做运动目标跟踪 OpenCV学习笔记（三十七）——实用函数、系统函数、宏core OpenCV学习笔记（三十八）——显示当前FPS OpenC
OpenCV学习笔记(三十六)--Kalman滤波做运动目标跟踪 kalman滤波大家都很熟悉,其基本思想就是先不考虑输入信号和观测噪声的影响,得到状态变量和输出信号的估计值,再用输出信号的估计误差加 ...
最新文章
- WPF/E CTP Quick Start - 第三部分：Canvas对象（翻译）
- 网络爬虫--SAX处理xml
- Server 对象 错误 'ASP 0177 : 800401f3' Server.CreateObject 失败解决方案
- 扫地机器人返充原理_扫地机器人原理是什么？
- Starling框架帮助手册中文版（PDF下载）
- Spring缓存注解@Cache使用
- redis系列：redis介绍与安装
- HWPFDocument读取doc,wps文档（含图片读取）
- 解决用户意外退出在线列表无法及时更新问题2(转载)
- 案例分享：巧用工具提升无源码系统的性能和稳定性
- Poi读写Excel文件
- 后端MultipartFile接收文件转Base64
- 企业流程再造（BPR）
- 浙江理工大学本科毕业答辩beamer模板
- 武汉大学计算机学院宿舍楼名称,武汉大学寝室排名 寝室图片
- java计算机毕业设计南通大学福利发放管理系统源码+系统+数据库+lw文档+mybatis+运行部署
- 关于物联网的误区，你有没有中招？
- 美团leaf生成分布式唯一id
- NP问题真的很难理解
- 腾讯地图经纬度解析出地址