python 卡尔曼滤波多维时间序列卡尔曼滤波程序

转载

我是数据分析师 2024-02-03 06:56:31

文章标签 python 卡尔曼滤波多维时间序列 KalmanFilter 卡尔曼滤波算法 C语言陀螺仪 文章分类 Python 后端开发

文章目录

前言
学习目录
1、卡尔曼线性滤波的五条黄金公式
2、陀螺仪的原始数据
3、C语言源码分析
附录

1、矩阵乘法
2、协方差矩阵
3、单位矩阵

前言

前面的文章系统介绍了卡尔曼滤波算法的数学原理，接下来介绍如何用C语言实现卡尔曼滤波算法。

本文借鉴了 https://wenku.baidu.com/view/3c42b7733186bceb18e8bb29.html

1、卡尔曼线性滤波的五条黄金公式

2、陀螺仪的原始数据

python 卡尔曼滤波多维时间序列卡尔曼滤波程序_卡尔曼滤波算法_06

GYROSCOPE SENSITIVITY：

从MPU-6500数据手册中的陀螺仪规格说明表中可以看出陀螺仪的量程（Full-Scale Range）的单位为°/s。而其ADC的精度（Gyroscope ADC Word Length）为16位也就意味着数据寄存器的输出是范围是-7FFF~7FFF ,也既是-32767~32767，对应于-2000~2000°/s（此时FS_SEL=3）。

下方的灵敏度比例因子（Sensitivity Scale Factor）参数是选择不同量程的每单位物理计量值所映射数据寄存器数据的多少。比如在量程为-2000~2000°/s时对应为32767/2000=16.4LSB(°/s)

3、C语言源码分析

推荐使用Notepad++等代码编辑器软件查看注释。
下面的代码中包含的卡尔曼滤波函数的输入值为陀螺仪的测量值和加速度计的角度计算值，函数将陀螺仪的测量值和角速度零漂作为估计值，加速度计的角度计算值作为测量值，来计算最优角度值。

//--------------------实现加速度计和陀螺仪数据融合计算角度
float Angle=0.0						//卡尔曼滤波器的输岀值，最优估计的角度
//float Gyro_x=0.0;					//卡尔曼滤波器输的出值，最优估计的角速度
float Q_angle=0.001; 				//陀螺仪角度噪声协方差
float Q_gyro=0.003; 				//陀螺仪漂移噪声协方差
float R_angle=0.5;					//加速度计测量噪声协方差
float dt=0.005;						//积分时间（滤波器采样时间）
char  C_0=1;						//H矩阵的一个数
float Q_bias=0,Angle_err=0;			//Q_bias为陀螺仪漂移
float PCt_0=0,PCt_1=0,E=0;			//中间变量
float K_0=0,K_1=0,t_0=0,t_1=0;		//K是卡尔曼增益，t是中间变量
float Pdot[4] = {0,0,0,0};			//计算P矩阵的中间变量
float PP[2][2]={	{1,0},			//公式中P矩阵即X的协方差
					{0,1}	};
					
void Kalman_Filter(float Gyro, float Accel)//Gyro陀螺仪的测量值，Accel为加速度计的角度计算值	
{
//Prediction:
	/*
		X(k|k-1)=AX(k-1|k-1)+BU(k)
		|Angle(k)|	 |1  -dt| | Angle(k-1)|   | dt |
		|	     | = |	    | |           | + |    |Gyro
		|Q_bisas |	 |0    1| |  Q_bisas  |   |  0 |
	*/
	Angle += (Gyro-Q_bias)*dt
	//Q_bias=Q_bias
	//角度测量模型方程，角度估计值=上一次的最优角度+（角速度-上一次的最优零漂）dt
	//就漂移来说认为每次都是相同的 Q_bias=Q_bias
	
	/*
		P(k|k-1)=AP(k-1|k-1)A'+Q (Q为向量A的协方差矩阵)
		因为陀螺仪的漂移噪声和角度噪声相互独立所以cov(Angle,Q_bias)=0,cov(Q_bias,Angle)=0
			|cov(Angle,Angle)	cov(Q_bias,Angle)	|	|Q_Angle	  0   |					 |a	b|	
		Q = |										| = |				  |	设：P(k-1|k-1) = |	 |
			|cov(Angle,Q_bias)	cov(Q_bias,Q_bias)	|	|	0		Q_gyro|					 |c	d|
																
												 d*(dt)^2数值过小忽略
					|a-c*dt-b*dt+d*(dt)^2 	b-d*dt	|----------->|a-(c+b)dt		b-d*dt	|
		P(k|k-1) = 	|								| + Q = 	 | 						| + Q
					|          c-d*dt		  d		|			 | c-d*dt		  d		|
		下方的计算中将Q_Angle、Q_gyro也同时乘以dt
		因为Q_angle、Q_gyro和dt在公式中都是固定值
		所以只要初始值设置合适就没有问题。			
	*/
	Pdot[0] = Q_angle - PP[0][1] -PP[1][0];
	Pdot[1] = -PP[1][1];
	Pdot[2] = -PP[1][1];
	Pdot[3] =  Q_gyro;
	
	PP[0][0] += Pdot[0] *dt;
	PP[0][1] += Pdot[1] *dt;
	PP[1][0] += Pdot[2] *dt;
	PP[1][1] += Pdot[3] *dt;
//Updata:
	/*
		Kg(k)= P(k|k-1)H'/(HP(k|k-1)H'+R) 
			|K_0|										
		Kg =|	|(对应于Angle和Q_bias的卡尔曼增益) 	H = |1 0| →↓
			|K_1|									(系数矩阵，因为在卡尔曼滤波的推导公式中有量测方程
													 Z(k)=Hx(k)+v(k),Z为观测变量,也就是程序中输入的角
													 度测量值Accel，Angle和Accel是直接相关的而Q_bias
													 和Accel无关所以H = |1 0|)	
						|a b|					|a b| |1|				  |a b||1|
		设PP即P(k|k-1)= |	|		P(k|k-1)H'= |	|*| |=|a c|;HPH'=|1 0||	  || |=a
						|c d|					|c d| |0|				  |c d||0|
						其中|a c|对应PCt_0和PCt_1
	*/
	PCt_0 = C_0 * PP[0][0];		//矩阵乘法的中间变量
	PCt_1 = C_0 * PP[1][0];		//C_0=1
	E = R_angle + C_0* PCt_0;	//分母
	K_0 = PCt_0 / E;			//Angle卡尔曼增益
	K_1 = PCt_1 / E;			//Q_bis卡尔曼增益
	/*
		X(k|k)= X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))
	*/
	Angle_err = Accel- Angle;
	Angle += K_0 * Angle_err;	//计算最优角度
	Q_bias += K_1 * Angle_err;	//计算最优零漂
	//Gyro_x= Gyro - Q bias；	//计算得最优角速度
	/*
		P(k|k)=(I-Kg(k)H)P(k|k-1)
			|1 0|
		I = |	| 所以根据单位矩阵的性质：P(k|k)=P(k|k-1)-Kg(k)*H*P(k|k-1)
			|0 1|
							|K_0| (		  |a b| ) 	|K_0*a K_0*b|
		Kg(k)*H*P(k|k-1) = 	|	|*( |1 0|*|	  | ) = |			|
							|K_1| (		  |c d| )	|K_1*a K_1*b|
	*/
	t_0 = PCt_0;				//矩阵计算中间变量，相当于a
	t_1 = C_0 * PP[0][1];		//矩阵计算中间变量，相当于b
	
	PP[0][0] -= K_0 * t_0;
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;	
}