卡尔曼滤波 java 卡尔曼滤波matlab

学习参考：

卡尔曼滤波器的原理以及在matlab中的实现

Opencv实现Kalman滤波器

opencv中的KF源码分析

Opencv-kalman-filter-mouse-tracking

理解：

假设：一个小车距离左侧某一物体k时刻的真实位置状态

,而位置状态观测值为

,则小车的线性动态系统可表示为：

位置状态的系统预测值：

位置状态的观测值：

其中F为系统状态转移矩阵，B为控制输入矩阵，H为系统状态观测矩阵，

为系统预测过程中的噪声（其均值为0，协方差为Q），

为系统测量过程中的噪声（其均值为0，协防方差为R）.

则卡拉曼滤波的思想就是利用Kalaman增益修正预测值

使其逼近真实值

来计算最优估计值

。

Kalaman步骤：

1. 通过上一时刻的估计值预测当前状态的估计值
2. 计算先验误差估计协方差矩阵
3. 计算此时卡尔曼增益
4. 更新估计值
5. 计算后验误差估计协方差
6. ,循环1-5。

Kalaman优点：

• 时域滤波器，不需变换到频域设计
• 一种递归估计，不需要记录历史观测值和估计值

Kalaman的实现：

• kalaman.hpp

namespace cv{


class CV_EXPORTS_W KalmanFilter
{
public:
    //缺省构造函数：无论构造函数是自动生成的还是用户定义的，它都是可以调用的构造函数，无需提供任何参数。
    CV_WRAP KalmanFilter();
    //完整构造函数
    CV_WRAP KalmanFilter(int dynamparams,int measureparamsint, int controlParams=0, int type=CV_32F);  
    //dynamparams:状态维度
    //measureparamsint：观测维度
    //controlParams：控制维度
    void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);

    //计算预测状态
    CV_WRAP const Mat& predict(const Mat& control=Mat());  
    //依据观测值更新预测状态 
    CV_WRAP const Mat& correct(const Mat& measurement);  
  
    Mat statePre;           //!< 预测状态 (x'(k)): x(k)=A*x(k-1)+B*u(k)  
    Mat statePost;          //!< 纠正状态 (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))  
    Mat transitionMatrix;   //!< 状态转换矩阵 (F)  
    Mat controlMatrix;      //!< 控制矩阵(B) (not used if there is no control)  
    Mat measurementMatrix;  //!< 观测矩阵(H)  
    Mat processNoiseCov;    //!< 处理过程噪声协方差矩阵 (Q)  
    Mat measurementNoiseCov;//!< 观测噪声协方差矩阵 (R)  
    Mat errorCovPre;        //!< 先验误差估计协方差矩阵 (P'(k)): P'(k)=A*P(k-1)*At + Q)*/  
    Mat gain;               //!< 卡尔曼增益矩阵 (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)  
    Mat errorCovPost;       //!< 后验误差估计协方差矩阵 (P(k)): P(k)=(I-K(k)*H)*P'(k)  
      
    // temporary matrices  
    Mat temp1;  
    Mat temp2;  
    Mat temp3;  
    Mat temp4;  
    Mat temp5;  
};
}

• kalaman类的实现

namespace cv
{
KalmanFilter::KalmanFilter() {}
    //dynamparams:状态维度
    //measureparamsint：观测维度
    //controlParams：控制维度
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type)
{
    init(dynamParams, measureParams, controlParams, type);
}
 
void KalmanFilter::init(int DP, int MP, int CP, int type)
{
    CV_Assert( DP > 0 && MP > 0 );
    CV_Assert( type == CV_32F || type == CV_64F );
    CP = std::max(CP, 0);
 
    statePre = Mat::zeros(DP, 1, type);//状态向量维度DP X 1
    statePost = Mat::zeros(DP, 1, type);
    transitionMatrix = Mat::eye(DP, DP, type);// 状态转移矩阵F的大小为DP X DP
 
    processNoiseCov = Mat::eye(DP, DP, type);// 处理过程噪声协方差矩阵Q的大小为DP X DP
    measurementMatrix = Mat::zeros(MP, DP, type);//观测矩阵H的大小为MP X DP
    measurementNoiseCov = Mat::eye(MP, MP, type);// 观测噪声协方差矩阵R大小为MP X MP
 
    errorCovPre = Mat::zeros(DP, DP, type); // 先验误差估计协方差矩阵P'大小为DP X DP
    errorCovPost = Mat::zeros(DP, DP, type);// 后验误差估计协方差矩阵P大小为DP X DP
    gain = Mat::zeros(DP, MP, type); // 卡尔曼增益矩阵
 
    if( CP > 0 )
        controlMatrix = Mat::zeros(DP, CP, type);// 控制矩阵B
    else
        controlMatrix.release();
 
    temp1.create(DP, DP, type);
    temp2.create(MP, DP, type);
    temp3.create(MP, MP, type);
    temp4.create(MP, DP, type);
    temp5.create(MP, 1, type);
}
 
const Mat& KalmanFilter::predict(const Mat& control)
{
    // 更新预测状态 e: x'(k) = F*x(k)
    statePre = transitionMatrix*statePost;
    //判断是否添加控制，是否添加控制矩阵影响更新的预测状态
    if( !control.empty() )
        // x'(k) = x'(k) + B*u(k)
        statePre += controlMatrix*control;
 
    // temp1 = F*P(k)
    temp1 = transitionMatrix*errorCovPost;
 
    // 更新先验误差估计协方差矩阵P'(k) = temp1*F_T + Q
	// gemm(a,b,n,c,m,ans,flags): ans = n*a*b+m*c
    // flags=GEMM_2_T:b transposes
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
 
    // handle the case when there will be measurement before the next predict.
    //a.copyTo(b):得到与a一样的b，两者可进行互不相关的操作。
    statePre.copyTo(statePost);
    errorCovPre.copyTo(errorCovPost);
 
    return statePre;
}
 
const Mat& KalmanFilter::correct(const Mat& measurement)
{
    // K(k) = p'(k)*H_T*inv(H*p'(k)*H_T+R)
    // temp2 = F*P'(k)
    temp2 = measurementMatrix * errorCovPre;
 
    // temp3 = temp2*H_T + R
    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
    //
    // solve(MA,MB,MX,CV_LU) 可以求AX=B for X 等价于X=A-1 *B
    // temp4 = inv(temp3)*temp2 = K(k)_T
	solve(temp3, temp2, temp4, DECOMP_SVD);
 
    // K(k)
	// 转置过来得到真正的K
    gain = temp4.t();
 
    // temp5 = z(k) - H*x'(k)
    temp5 = measurement - measurementMatrix*statePre;
 
    // x(k) = x'(k) + K(k)*temp5
    statePost = statePre + gain*temp5;
 
    // P(k) = P'(k) - K(k)*temp2
    errorCovPost = errorCovPre - gain*temp2;
 
    return statePost;
}
}

• demo.cpp

#include "opencv2/video/tracking.hpp"
#include <iostream>
#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace cv;
using namespace std;
void on_mouse(int e, int x, int y, int d, void *ptr)
{
	Point*p = (Point*)ptr;
	p->x = x;
	p->y = y;
}

int main()
{
	// 初始化
	int stateSize = 4;  // [x, y, v_x, v_y]
	int measSize = 2;   // [z_x, z_y] 
	int contrSize = 0;  // no control input
	unsigned int F_type = CV_32F;//or CV_64F

	KalmanFilter KF(stateSize, measSize, contrSize, F_type);
	Mat state(stateSize, 1, F_type);  // [x, y, v_x, v_y] 
	Mat meas(measSize, 1, F_type);    // [z_x, z_y] 
	setIdentity(KF.transitionMatrix);
	setIdentity(KF.measurementMatrix);
	setIdentity(KF.processNoiseCov, Scalar::all(1e-4));
	setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
	setIdentity(KF.errorCovPost, Scalar::all(.1));


	Mat display_image(600, 800, CV_8UC3);
	namedWindow("Mouse Kalman");

	char ch = 0;
	double ticks = 0;
	Point mousePos;
	vector<Point> mousev, kalmanv;

	while (ch != 'q' && ch != 'Q')
	{
		//预测
		state = KF.predict(); 

		Point predictPt(state.at<float>(0), state.at<float>(1));
	
		// 鼠标观测
		setMouseCallback("Mouse Kalman", on_mouse, &mousePos);
		meas.at<float>(0) = mousePos.x;
		meas.at<float>(1) = mousePos.y;

		// >更新
		Mat estimated = KF.correct(meas);

		Point statePt(estimated.at<float>(0), estimated.at<float>(1));
		Point measPt(meas.at<float>(0), meas.at<float>(1));
		char mousep[20];
                sprintf(mousep, "(%0.2f,%0.2f)", meas.at<float>(0), meas.at<float>(1));
                putText(display_image, "mouse point:",Point(0,10), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );
                putText(display_image, mousep, Point(120,10), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false);
		char prept[20];
                sprintf(prept, "(%0.2f,%0.2f)", state.at<float>(0), state.at<float>(1));
                putText(display_image, "pre point:",Point(0,50), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );
                putText(display_image, prept,Point(120,50), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 0), 2,false );
 

		imshow("Mouse Kalman", display_image);
		display_image = Scalar::all(0);
		//push_back 将新元素移动拷贝到vector里
		mousev.push_back(measPt);
		kalmanv.push_back(statePt);

		//以点坐标画园
		circle(display_image,statePt,5,(255,0,0),1);
		circle(display_image,measPt,5,(0,0,255),1);

		//画出运动轨迹
		for (int i = 0; i < mousev.size() - 1; i++)
			line(display_image, mousev[i], mousev[i + 1], Scalar(0, 0, 255), 1);

		for (int i = 0; i < kalmanv.size() - 1; i++)
			line(display_image, kalmanv[i], kalmanv[i + 1], Scalar(255, 0, 0), 1);


		ch = cv::waitKey(10);
	}
}