写在前面

Kalman滤波的理论是比较有深度的,涉及到很多公式推导,尤其是用到了很多线性代数的知识。

这篇博客是博主基于工作内容实现的,所以主要偏向于应用,代码也是经过无数次验证了的,是比较稳定的,大家可以放心使用,有问题可以留言交流。

关于更深层次的理论推导,这里将不会涉及,但是会推荐几个写的不错的博客供大家参考。

Kalman滤波基本原理

思想

Kalman的思想还是很简洁的, 标准卡尔曼滤波的核心思想是:假定观测的系统是线性的,噪声都满足高斯分布。这一刻系统的状态( 最优估计)是这一刻的 预测值和这一刻的 测量值的加权平均, 迭代给出包含噪声(协方差矩阵)最小的结果。

举个栗子:一个装有GPS的机器人在路上行走,GPS对机器人的位置、方向或者速度有一个 测量值,当然GPS也不是准确的,存在一定精度误差(噪声);然后机器人本身会根据过去机器人的位置、速度会对机器人当前的状态进行预测(比如用上一时刻速度预测下一时刻的速度),得到一个 预测值当然预测也不是完全准确的(噪声)。所以Kalman的方法就是对预测值和测量值进行加权融合,找到预测值和测量值的权重,使得输出结果是最准确的(噪声最小)。

更形象的例子: 来自 https://my.oschina.net/u/4353069/blog/4481449
 

输出值 = 0.5 * 测量值 + 0.5 * 预测值

如果这个测量值噪声有点大,也就是充满不确定性,怎么办?那就三七开吧,相信预测值多一些:

输出值 = 0.3 * 测量值 + 0.7 * 预测值

咱换一个写法:

输出值 = 0.3 * 测量值 + (1-0.3) * 预测值

            = 预测值  + 0.3 *(测量值 - 预测值)

            = 预测值  + K *(测量值 - 预测值)

这样我们用一个参数K就可以表示我们对数据的偏好(参数K越大,表示相信测量值越多)。

实际上,估计值和测量值都存在不确定性,所以这个参数的选择是有标准的:希望输出值的不确定性最小。

卡尔曼滤波算法的本质,就是基于数值的不确定性调节这个参数K(卡尔曼增益)。在迭代若干次后逼近输出值不确定性最小的数值。

需要解决4个问题

  1. 如何获得预测值(测量值是系统给定的,如GPS提供的,也就是已知的)
  2. 如何表示预测的不确定性(预测值的噪声)
  3. 如何获得卡尔曼增益(参数K)
  4. 如何完成迭代

1 如何获得预测值

以滑轨上的小车为例,知道上一次的位置x,速度v和加速度a,可以估计下一次的位置在:

     

h是步长,也就是时间间隔。为了方便后面公式表达,把它写成状态方程的形式

速度和位置都在状态中:

加速度放在控制量矩阵中:

系数矩阵A:

系数矩阵B:

写成矩阵形式:

观测方程

Z是测量值,矩阵H是测量值和真实状态之间的关系,对于直接测量的系统,H为单位阵,对于间接测量的系统,则需要H实现映射。比如状态是距离,测量使用的传感器是激光ToF测距传感器(这种传感器通过获得光信号在介质中的传播时间来计算发射位置到反射面的距离),也就是测量了光在二倍距离下传播的时间。那么H矩阵就应该是介质光速倒数的两倍 2c^-1 。

说明一下:控制矩阵不是必须,你也可以把上面的加速度放在系数矩阵A中,这样就没有控制矩阵。

 

2 如何表示预测的不确定性(预测值的噪声)

现在我们进入了真实环境:无论是估计还是测量,都带有高斯分布的噪声(W和V):

估计噪声 W 和量测噪声 V 满足高斯分布

Q,R 为估计噪声 W 和量测噪声 V 的方差。

现在要做的就是求出一个参数K,融合先验的状态估计值Xt和测量值Zt,使最终的结果中包含噪声W,V的成分最少。如何衡量数据中噪声成分的多少呢?我们引入了协方差矩阵的概念,对于预测值,协方差矩阵为:

其中前面一部分:来自上一次的估计,上一次的数据就有不确定性(噪声),基于上次数据做的预测,必然带有上次的不确定性,此外预测本身就引入了新的噪声,我们认为这是固定的,也就是。如此一来,我们就得到了预测值的不确定性。

再来看测量值的不确定性也就是观测噪声的方差,是多少呢?一个常数——,好嘛真开心,遇到一个不要计算的,为啥?传感器得到的数据,误差就是不变的呀,随便找一个传感器,厂家都会在手册上写的清清楚楚:测量误差是多少,这是一个固定的数据。

 

3 如何获得卡尔曼增益(参数K)

如此一来,参数K的取值就毫无悬念了:

卡尔曼滤波输出的最终状态为: 

根据以上两个公式可以看到,如果传感器的不确定性为0(仿真中的理想传感器),那么卡尔曼增益参数K将变成单位阵(假设H为单位阵),也就是输出值等于测量值。完全不看估计值了。在假设上次的估计的数值非常准确,给出的不确定度为0(比如触发了某个事件,如电机转动到了触发限位开关的特定角度),那么估计值的不确定性就只有Q,卡尔曼增益K因此下降,使得本次输出更加相信估计值。

 

4  如何完成迭代

最后一步,更新本次输出值的不确定性:

特别注意不一样,P拔(P-bar)的估计值的协方差矩阵(不确定性),P是本次滤波输出值的协方差矩阵(不确定性)。

而本次输出的协方差矩阵(不确定性),就是下一次计算估计值协方差矩阵时使用的“上一次数值的协方差矩阵(不确定性)。如此便完成了迭代

5 卡尔曼滤波五步走

现在把卡尔曼滤波的五个公式一起列出:

预测:
公式1: 【先验估计的状态向量】, 基于过去时刻的状态以及控制量对当前时刻的状态进行估计,注意,数据是先验的。
 
公式2:  【 先验估计的状态向量的协方差矩阵】, 计算 公式1 的先验估计状态的协方差矩阵,前一部分是由上次最优估计值自身引入的协方差,后一部分是本次估计产生的不确定性(比如无法预测的外部扰动以及模型自身的不确定性)。
公式3: 【卡尔曼增益】 , 计算卡尔曼增益(参数K),这个参数决定了相信估计值还是相信测量值多一些。
 
矫正:
公式4: 【最优的状态估计】 就是根据 公式4 的卡尔曼增益计算得到的最优状态估计值
 
公式5: 【最优的状态估计的协方差】 计算本次最优状态估计值的协方差矩阵,这个数据在下一次迭代的 公式2 中被用到。

以上内容均来自:https://my.oschina.net/u/4353069/blog/4481449。我做了些许的改动和补充。关于更深层的推导也可以看: https://my.oschina.net/u/4353069/blog/4481449。

可以发现,卡尔曼滤波就是两部分(预测和矫正),一共5个公式,代码实现只要按照5个公式实现就可以了。

代码实现

这里分了两个版本:一个c++版本和一个c版本。

实现Kalman滤波会涉及到大量的矩阵运算,所以要么使用矩阵库(如Eigen),要么就自己实现一个。

这里C++的版本使用了Eigen库,Eigen库的配置看这里:https://blog.csdn.net/weixin_40647819/article/details/110946546;

C版本是自己实现的。大家可以选择使用。

另外值得说明一下,这里不仅实现了标准卡尔曼滤波,还实现了一个自适应卡尔曼滤波

自适应卡尔曼滤波论文:

Tracking Touched Trajectory on Capacitive Touch Panels Using Adjustable Weighted Prediction Covariance Matrix
翻译:基于可调加权预测协方差矩阵的电容式触摸屏接触轨迹跟踪

这个自适应卡尔曼滤波引入了一个参数beta,用于调节预测噪声,当然这是触控领域的应用,如果是其他应用,请大家自己根据需求修改参数。

kalman.h

/*
@by: Wang Cheng Wei
@2020.12
@reference resources :
@https://blog.csdn.net/sandalphon4869/article/details/103841275?utm_medium=distribute.pc_relevant.none-task-blog-title-2&spm=1001.2101.3001.4242
@https://blog.csdn.net/lemonaha/article/details/73194763
*/
 
#ifndef  KF_H
#define  KF_H
 
#include <Eigen/Dense>
using namespace Eigen;
 
class Kalman{
 
public:
	/*
	@breif Standard Kalman filter
	*/
	MatrixXf &predict(); 
 
	/*
	@brief AFK: "Tracking Touched Trajectory on Capacitive Touch Panels Using Adjustable Weighted Prediction Covariance Matrix."
	@param  vx Velocity in x-axis direction
	@param  vy Velocity in y-axis direction
	@param  yetax  Error in x-axis direction :z(k)-H*x'(k)
	@param  yetay  Error in y-axis direction :z(k)-H*x'(k)
	*/
	MatrixXf &AK_predict(double &vx, double &vy, double &yetax, double &yetay); 
 
	/*
	@brief Adactive velocit adjust 
	@param  vx Velocity in x-axis direction
	@param  vy Velocity in y-axis direction
	@param  yetax  Error in x-axis direction(innovation sequence):z(k)-H*x'(k)
	@param  yetay  Error in y-axis direction(innovation sequence):z(k)-H*x'(k)                              
	@V_L Low threshold of speed
	@V_H High threshold of speed
	*/
	MatrixXf &AKV_predict(double vx, double vy, double yetax, double yetay, double V_L,double V_H); 
	
	/*
	@brief  STFK: "Position stimation and Smooth Tracking with a Fuzzy Logic-Based Adaptive Strong Tracking Kalman Filter for Capacitive Touch Panels."
	@param  measureParams Dimensionality of the measurement.
	@param  vx Velocity in x-axis direction
	@param  vy Velocity in y-axis direction
	@param  yetax  Error in x-axis direction() :z(k)-H*x'(k)
	@param  yetay  Error in y-axis direction :z(k)-H*x'(k)
	*/
	MatrixXf &STFK_predict(int measureParams, double &vx, double &vy, double &yetax, double &yetay);
 
	// updates the predicted state from the measurement
	MatrixXf &correct(MatrixXf &measurement);
 
	// updates the predicted state from the measurement
	MatrixXf &AKV_correct(MatrixXf measurement,double vx, double vy);
 
 
	/* 
	@brief Re-initializes Kalman filter.
	@param dynamParams Dimensionality of the state.
	@param measureParams Dimensionality of the measurement.
	@param controlParams Dimensionality of the control vector.
	*/
	void init(int dynamParams, int measureParams, int controlParams = 0);
 
	MatrixXf statePre; //predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
	MatrixXf stateOpt; //corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
	MatrixXf transMat; //state transition matrix (A)
	MatrixXf transMat_slow; //state transition matrix (A)
	MatrixXf contrMat; // control matrix (B) (not used if there is no control)
	MatrixXf measureMat; // measurement matrix (H)
	MatrixXf processNoiseCov; // process noise covariance matrix (Q)
	MatrixXf processNoiseCov_Slow; // process noise covariance matrix (Q)  SLOW speed
	MatrixXf processNoiseCov_Fast; // process noise covariance matrix (Q)  Fast speed
	MatrixXf measureNosiseCov; // measurement noise covariance matrix (R)
	MatrixXf measureNosiseCov_Slow; // measurement noise covariance matrix (R) SLOW speed
	MatrixXf measureNosiseCov_Fast; // measurement noise covariance matrix (R) Fast speed
	MatrixXf errorCovpre; // priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)
	MatrixXf Kgain;        //  Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
	MatrixXf errorCovOpt;  //   posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
 
	//STKF
	MatrixXf Vk;   
	MatrixXf Nk;
	MatrixXf Mk;
	double rho; //forgetting factor
	double get_lamda(int MP); // calculation suboptimal scaling factor
	double lamda; //suboptimal scaling factor
 
	//AKF
	double get_beta(double vx, double vy, double yetax, double yetay); //calculation beta
	double get_beta_plus(double vx, double vy); //calculation beta
	double eps;  //A empirically param determined  0.01
	double beta; //A wight is used to adjust the prediction convariance matrix of AKF
 
	//temporary Mat
	MatrixXf tmp1;
	MatrixXf tmp2;
	MatrixXf tmp3;
	MatrixXf tmp4;
	MatrixXf tmp5;   //innovation sequence vk
 
private:
	//STKF
	MatrixXf &get_Vk();
	MatrixXf &get_Mk();
	MatrixXf &get_Nk();
};
 
#endif

kalman.cpp 

#include <iostream>
#include <Eigen/Dense>
#include "Kalman.h"
using namespace Eigen;
using namespace std;
 
void Kalman::init(int DP, int MP, int CP){
	assert(DP > 0 && MP > 0);
	CP = std::max(CP, 0);
 
	statePre = MatrixXf::Zero(DP, 1);   //predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
	stateOpt = MatrixXf::Zero(DP, 1);   //corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
	transMat = MatrixXf::Identity(DP, DP); //A
	transMat_slow = MatrixXf::Identity(DP, DP); //A
	processNoiseCov = MatrixXf::Identity(DP, DP); //Q
	processNoiseCov_Slow = MatrixXf::Identity(DP, DP); //Q Slow_speed
	processNoiseCov_Fast = MatrixXf::Identity(DP, DP); //Q Fast_speed
	measureMat = MatrixXf::Zero(MP, DP); //H
	measureNosiseCov = MatrixXf::Identity(MP, MP); //R
	measureNosiseCov_Slow = MatrixXf::Identity(MP, MP); //R Slow_speed
	measureNosiseCov_Fast = MatrixXf::Identity(MP, MP); //R Fast_speed
	errorCovpre = MatrixXf::Zero(DP, DP);   //(P'(k)): P'(k)=A*P(k-1)*At + Q)
	errorCovOpt = MatrixXf::Zero(DP, DP);   //(P(k)) : P(k) = (I - K(k)*H)*P'(k)
	Kgain = MatrixXf::Zero(DP, MP);  //(K(k)) : K(k) = P'(k)*Ht*inv(H*P'(k)*Ht + R)
 
	//STKF
	Vk = MatrixXf::Zero(MP, MP);
	Nk = MatrixXf::Zero(MP, MP);
	Mk = MatrixXf::Zero(MP, MP);
	rho = 0.55;
 
	//
	if (CP > 0)
		contrMat = MatrixXf::Zero(DP, CP);
 
	tmp1 = MatrixXf::Zero(DP, DP);
 
	//tmp2 = MatrixXf::Zero(MP, DP);
	tmp2 = MatrixXf::Zero(DP, MP);
	tmp3 = MatrixXf::Zero(MP, MP);
	//tmp4 = MatrixXf::Zero(MP, DP);
	tmp4 = MatrixXf::Zero(DP, MP);
	tmp5 = MatrixXf::Zero(MP, 1);
}
 
double Kalman::get_beta(double vx, double vy, double yetax, double yetay){
 
	double yeta = abs(vx + vy + yetax + yetay) / 25;
	//cout << yeta << endl;
	
	double bt1 = 0.001 + 1 / (1 + exp(-(yeta - 5)));
	double bt2 = 0.001 + 1 / (1 + exp(-(yeta - 8)));
	double bt3 = 0.001+ 1 / (1 + exp(-(0.5*yeta - 5)));
	double bt4 = 0.001 + 1 / (1 + exp(-(0.4*yeta - 5)));
	double bt5 = 0.001 + 1 / (1 + exp(-(0.85*yeta - 8)));
 
	return bt5;
}
 
double Kalman::get_beta_plus(double vx, double vy){
 
	double Vxy = sqrt(vx*vx + vy*vy);
	double bt=0;
	double tp1 = Vxy - 30;
	double tp2 = pow(Vxy - 20, 0.1);
	if (Vxy <= 30)
		bt =  0.0000006;
	else if (Vxy > 30 && Vxy <70)
		bt = 0.6 * (0.1 + 100 / (0.6 + exp(-tp1)));
	else if (Vxy >= 70 && Vxy<= 80)
		bt = 2 * (0.01 + 100 / (0.15 + exp(-tp2)));
	else if (Vxy >80)
		bt = 2* (0.01 + 100 / (0.1 + exp(-tp2)));
	//	bt = 0.1 * (0.01 + 100 / (0.1 + exp(-tp2)));
	/*if (Vxy <= 30)
		bt = 0.01;
	if (Vxy > 30 && Vxy < 80)
		bt = 0.5;
	if (Vxy >= 80)
		bt = 1.2;*/
 
	return bt;
}
 
MatrixXf& Kalman::get_Vk(){
 
	Vk = (rho*Vk + tmp5*tmp5.transpose()) / (1 + rho);
 
	return Vk;
}
 
MatrixXf& Kalman::get_Mk(){
 
	Mk = measureMat*transMat*errorCovpre * transMat.transpose() * measureMat.transpose();
	return Mk;
}
 
MatrixXf& Kalman::get_Nk(){
 
	Nk = Vk - measureNosiseCov - measureMat *processNoiseCov*measureMat.transpose();
	return Nk;
}
 
double Kalman::get_lamda(int MP){
	double tr_Nk = 0;
	double tr_Mk = 0;
	double lamda = 0;
	Vk=get_Vk();
	Nk=get_Nk();
	Mk=get_Mk();
	for (int i = 0; i < MP; ++i){
		   
		tr_Nk = tr_Nk + Nk(i, i);
		tr_Mk = tr_Mk + Mk(i, i);
	}
	double ck = tr_Nk /(tr_Mk+0.0000001);
	//cout << ck << endl;
	if (ck >= 1)
		lamda = ck;
	else if (ck < 1)
		lamda = 1;
	return lamda;
}
 
//强跟踪卡尔曼
MatrixXf &Kalman::STFK_predict(int MP, double &vx, double &vy, double &yetax, double &yetay){
 
	lamda = get_lamda(MP);
	//cout << lamda << endl;
 
	// update the state: x'(k) = A*x(k)
	statePre = transMat*stateOpt;
	// update error covariance matrices : tmp1 = A*P(k)
	tmp1 = transMat*errorCovOpt;
	// P'(k) = lamda * temp1*At + Q
	beta = get_beta(vx, vy, yetax, yetay);
	cout << beta *1<< endl;
	errorCovpre = lamda * tmp1*transMat.transpose() + beta * processNoiseCov;
 
	//iteration
	stateOpt = statePre;
	errorCovOpt = errorCovpre;
 
	return statePre;
}
 
 
 
//自适应卡尔曼
MatrixXf& Kalman::AK_predict(double &vx, double &vy, double &yetax, double &yetay){
 
	// update the state: x'(k) = A*x(k)
	statePre = transMat*stateOpt;
	// update error covariance matrices : temp1 = A*P(k)
	tmp1 = transMat*errorCovOpt;
	// P'(k) = temp1*At + Q
	beta = get_beta(vx, vy, yetax, yetay);
	//cout << beta *1 << endl;
	//beta = get_beta_plus(vx, vy);
	errorCovpre = tmp1*transMat.transpose() + beta * processNoiseCov;
 
	//iteration
	stateOpt = statePre;
	errorCovOpt = errorCovpre;
 
	return statePre;
 
}
 
 
//速度自适应卡尔曼
MatrixXf& Kalman::AKV_predict(double vx, double vy, double yetax, double yetay,double V_TL,double V_TH){
 
	// update the state: x'(k) = A*x(k)
	statePre = transMat*stateOpt;
	// update error covariance matrices : temp1 = A*P(k)
	tmp1 = transMat*errorCovOpt;
	// P'(k) = temp1*At + Q
	beta = get_beta(vx, vy, yetax, yetay);
	//cout << beta<< endl;
	
	double Vxy = sqrt(vx*vx + vy*vy);
	 
	if ( Vxy <= V_TL){
		errorCovpre = tmp1*transMat.transpose() + beta * processNoiseCov_Slow;
		cout << "slow" << endl;
	}	               
	  
	else if (Vxy>V_TL &&Vxy<V_TH){
		errorCovpre = tmp1*transMat.transpose() + beta * processNoiseCov;
		cout << "median" << endl;
	}
	else if (Vxy >= V_TH){
		errorCovpre = tmp1*transMat.transpose() + beta * processNoiseCov_Fast;
		
	}
 
	//iteration
	stateOpt = statePre;
	errorCovOpt = errorCovpre;
 
	return statePre;
 
}
 
 
//标准卡尔曼
MatrixXf& Kalman::predict(){
 
	// update the state: x'(k) = A*x(k)
	statePre = transMat*stateOpt;
	// update error covariance matrices : tmp1 = A*P(k)
	tmp1 = transMat*errorCovOpt;
	// P'(k) = temp1*At + Q
	errorCovpre = tmp1*transMat.transpose() + processNoiseCov;
 
	//iteration
	stateOpt = statePre;
	errorCovOpt = errorCovpre;
	
	return statePre;
 
}
 
 
MatrixXf& Kalman::correct(MatrixXf &measurement){
	// tmp2 = H*P'(k)
	//tmp2 = measureMat * errorCovpre;
	tmp2 = errorCovpre* measureMat.transpose();
	// tmp3 = tmp2*H + R
	//tmp3 = tmp2*measureMat.transpose() + measureNosiseCov;
	tmp3 = measureMat*tmp2 + measureNosiseCov;
	// tmp4 = inv(tmp3)*temp2 = Kt(k) /----/ (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
	//tmp4 = tmp3.inverse() * tmp2;
	tmp4 = tmp2 *tmp3.inverse();
	//K(k)
	//Kgain = tmp4.transpose();
	Kgain = tmp4;
	// tmp5 = z(k) - H*x'(k)
	tmp5 = measurement - measureMat * statePre;
 
	// x(k) = x'(k) + K(k)*temp5
	stateOpt = statePre + Kgain * tmp5;
	
	// P(k) = P'(k) - K(k)*H*P'(k)       
	//errorCovOpt = errorCovpre - Kgain *tmp2;
	errorCovOpt = errorCovpre - Kgain * measureMat * errorCovpre;
	 
	return stateOpt;
}
MatrixXf& Kalman::AKV_correct(MatrixXf measurement, double vx, double vy){
	double Vxy = sqrt(vx*vx + vy*vy);
	// tmp2 = H*P'(k)
	tmp2 = errorCovpre* measureMat.transpose();
	
	// tmp3 = tmp2*Ht + R
	tmp3 = measureMat*tmp2 + measureNosiseCov;
 
	// tmp4 = inv(tmp3)*temp2 = Kt(k) /----/ (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
	tmp4 = tmp2 *tmp3.inverse();
 
	//K(k)
	Kgain = tmp4;
 
	// tmp5 = z(k) - H*x'(k)
	tmp5 = measurement - measureMat * statePre;
	// x(k) = x'(k) + K(k)*temp5
	if (Vxy <=0 ){
		stateOpt(0, 0) = measurement(0, 0);//measurement(0, 0);
		stateOpt(3, 0) = measurement(1, 0);//measurement(1, 0);
	}
	else{
		stateOpt = statePre + Kgain * tmp5;
	}
 
	// P(k) = P'(k) - K(k)*H*P'(k)                                                                                             
	errorCovOpt = errorCovpre - Kgain * measureMat * errorCovpre;
 
	return stateOpt;
}

main.cpp

#include <stdio.h>
#include <Eigen/Dense>
#include<random>
#include<Eigen/Eigen>
#include<ctime>
#include "Windows.h"
#include "touch_ir.h"
#include "Kalman.h"
#include <Eigen\Dense>
#include <iostream>
 
using namespace Eigen;
using namespace std;
 
 
point			cp_xts_axis[LEDX];
point			cp_yts_axis[LEDY];
 
point  sour_data[26][16];
point  filt_data[26];
point  filt_data_pre[26];
point  tmp_data[26];
Kalman kf;
int ct = 0;
 
 
 
void Touch_Filter_Multi(void)
{
	u16 i = 0;
	MatrixXf measurement(2,1);
	//*
	double vx = abs(sour_data[i][0].x - sour_data[i][1].x); 
	double vy = abs(sour_data[i][0].y - sour_data[i][1].y);
	double vx1 = abs(sour_data[i][0].x - sour_data[i][12].x);
	double vy1 = abs(sour_data[i][0].y - sour_data[i][12].y);
 
 
		 //KF
	     vx = kf.stateOpt(1, 0);
		 vy = kf.stateOpt(4, 0);
		 double yetax = kf.tmp5(0, 0);
		 double yetay = kf.tmp5(1, 0);
	     measurement << filt_data[0].x, filt_data[0].y;
		 kf.predict();
		 //kf.AK_predict(vx, vy, yetax, yetay);
		 //kf.AKV_predict(vx, vy, yetax, yetay, 22, 100); //速度自适应卡尔曼
		 //kf.STFK_predict(2, vx, vy, yetax, yetay);
 
		kf.correct(measurement);
		 //kf.AKV_correct(measurement, vx1, vy1);
 
		 filt_data[0].x = kf.stateOpt(0, 0);
		 filt_data[0].y = kf.stateOpt(3, 0);
	   
}
 
 
 
 
int main(void)
{
	BOOL  init_f=FALSE;
	Init_Axis_Kd();
	CP_Pad_Size_Init_E2();
	Data_Copy_Proc(cp_padattv,cp_xts_axis,cp_yts_axis);
	Scan_Line_Init();
	Init_Mat_XValue_Debug();
	Init_Mat_YValue_Debug();
	StartMonitoring();
	init_f=TRUE;
 
	//KF 初始化
	kf.init(6,2);
	//A
	double t = 0.05;
	kf.transMat << 1, t, 0.5*t*t, 0, 0, 0,
				   0, 1, 0, 0, 0, 0,
				   0, 0, 1, 0, 0, 0,
				   0, 0, 0, 1, t, 0.5*t*t,
		           0, 0, 0, 0, 1, 0,
		           0, 0, 0, 0, 0, 1;
 
 
	//H
	kf.measureMat << 1, 0, 0, 0, 0, 0,
					 0, 0, 0, 1, 0, 0;
 
	//Q 
	double q = 1.2; //  
	double q1 = 0.00085, q2 = 98;
	kf.processNoiseCov << q, 0, 0, 0, 0, 0,
					      0, q, 0, 0, 0, 0,
		                  0, 0, q, 0, 0, 0,
		                  0, 0, 0, q, 0, 0,
		                  0, 0, 0, 0, q, 0,
		                  0, 0, 0, 0, 0, q;
 
	kf.processNoiseCov_Slow << q1, 0, 0, 0, 0, 0,
							   0, q1, 0, 0, 0, 0,
							   0, 0, q1, 0, 0, 0,
		                       0, 0, 0, q1, 0, 0,
		                       0, 0, 0, 0, q1, 0,
		                       0, 0, 0, 0, 0, q1;
 
	kf.processNoiseCov_Fast << q2, 0, 0, 0, 0, 0,
								0, q2, 0, 0, 0, 0,
								0, 0, q2, 0, 0, 0,
								0, 0, 0, q2, 0, 0,
								0, 0, 0, 0, q2, 0,
								0, 0, 0, 0, 0, q2;
 
	//R  
	double r = 1; //100 ~ 500
	double r_fast = 255;
	double r_slow = 100;
	kf.measureNosiseCov <<r, 0,
					    	0,r;
	kf.measureNosiseCov_Slow << r_slow, 0,
								0, r_slow;
	kf.measureNosiseCov_Fast << r_fast, 0,
								0, r_fast;
 
	//P
	kf.errorCovOpt << 1, 0, 0, 0, 0, 0,
				   	  0, 1, 0, 0, 0, 0,
					  0, 0, 1, 0, 0, 0,
		              0, 0, 0, 1, 0, 0,
		              0, 0, 0, 0, 1, 0,
		              0, 0, 0, 0, 0, 1;
 
	//
	kf.eps = 0.01;
	kf.rho = 1;
 
	while (1)
	{
		if(init_f==TRUE)
		{
			Get_XYShade_Group(&cp_com_va);
		}	
	}
	return 0;
}

注:可能代码看起来比较多而且杂,如果你只需要用标准kalman,那么你只需要关注这3个函数即可:

void init(int dynamParams, int measureParams, int controlParams = 0);

MatrixXf &predict(); 

MatrixXf &correct(MatrixXf &measurement);

main调用步骤:

1初始化

  • 首先,你必须实例化一个KF对象,然后调用kf.init(),初始化各个参数。
  • 根据自己的需求定义状态转移矩阵A
  • 定义状态测量矩阵H
  • 定义  预测噪声协方差矩阵Q和测量噪声协方差矩阵R
  • 初始化后验估计值协方差矩阵P

2预测

  • 调用predict();

3矫正

  • 传入测量值
  • 调用correct();
  • 输出结果。

C版本:因为代码行数多,所以不展开出来,我上传到了CSDN下载:https://download.csdn.net/download/weixin_40647819/14504744(只需1个积分),大家下载即可。

参数作用及调整

  • Q

Q 值为过程预测 噪声,越小系统越容易收敛,我们对模型预测的值信任度越高;但是太小则容易发散,如果 Q 为零,那么我们只相信预测值; Q 值越大我们对于预测的信任度就越低,而对测量值的信任度就变高;如果 Q 值无穷大,那么我们只信任测量值。

  • P

R 值为测量噪声,太小太大都不一定合适。 R 太大,卡尔曼滤波响应会变慢,因为它对新测量的值的信任度降低;越小系统收敛越快,但过小则容易出现震荡。

  • Q/R

测试时可以先将 Q 从小往大调整,将 R 从大往小调整;先固定一个值去调整另外一个值. 在实际的工程应用中, 只要固定估计噪声和测量噪声的方差的比值Q/R, 计算的结果就是一样的, 调整比值 Q/R  就可以改变滤波的效果,这个比值越大,越信任测量值。
 
 
推荐阅读

https://my.oschina.net/u/4353069/blog/4481449(解析、理论、推导)

https://zhuanlan.zhihu.com/p/37750839(参数解释)

https://blog.csdn.net/sandalphon4869/article/details/103841275?utm_medium=distribute.pc_relevant.none-task-blog-title-2&spm=1001.2101.3001.4242

(如果懂opencv的话,可以看看这篇opencv的Kalman滤波源码解析)