Kalman_Filter卡尔曼滤波器计算，陀螺仪卡尔曼滤波角度估算及代码

1.向量轴的空间角度角度计算

以横滚角为例，X轴旋转需要一个初始角度，Y、Z轴都会跟随X轴旋转而转动，我们认为Y轴平行于水平面时，横滚角Roll的角度为0。
从X轴观测，假设Y轴由水平面转动θ角度，则：

accY和accZ是Y、Z轴的加速度值。
从开始水平位置算起，随着θ的变大accY的数值增大（垂直分量增加）accZ的数值减小（垂直分量减小）；
注：这里面与X轴是否水平无关，X轴处于非水平状态，Y、Z平面对于垂直坐标轴的投影会在计算时被消除。

同样地：俯仰角pitch角度α

2.正态分布

正态分布，是一种连续型概率分布。在统计学中，正态分布是最常见的分布之一，许多自然和社会科学现象都可以用正态分布来描述。
正态分布的概率密度函数是一个钟形曲线，曲线中心位于均值（μ），曲线的宽度由标准差（σ）决定。正态分布的曲线是对称的，均值、中位数和众数都位于曲线的中心。
正态分布可以表示为：

在：

的区间内，概率分布占比68.3%，标准差（σ）越小（方差也越小），则事件概率集中度越高，越接近平均值（μ），则数据波动越小。
从另一个角度观察，选取一个标准差（或方差）分布比较小的数据时，数据中的参数比较容易接近真实值。

3.方差、协方差

方差
方差是一种用来衡量一组数据离散程度的统计量。简单来说，方差越大，数据的离散程度就越高；方差越小，数据的离散程度就越低。
方差公式：

Xi为数据点，μ表示数据的平均值，N表示样本数；
方差>=0,表示数据的离散程度。

协方差
协方差是一个用于度量两个随机变量之间线性关系强度和方向的统计量。
协方差公式：

Xi，Yi为数据点，μ表示数据的平均值，N表示样本数；
如果两个变量的变化趋势一致，也就是说如果其中一个变量大于其自身的期望值，另一个变量也大于其自身的期望值，那么这两个变量的协方差就是正的。 相反，如果两个变量的变化趋势相反，那么它们的协方差就是负的。
根据协方差的性质在某些条件下可以推导出估计值的相关系数，也就是权重；

协方差方程中因子之间的相关性较大时，权重（相关系数）接近于1，也就是得到的结果取测量值的比例更大；
反之，相关系数较小时接近于0，得到的结果取估计值比例更大。

4.卡尔曼公式计算

以下，以Roll横滚角为例。

4.1状态空间方程

状态空间方程:

也就是：

当前角度估计值=上一时刻角度估计值+角速度x积分时间+误差值
也可以写成：

此时，ω记为角速度的误差；
则，状态空间方程：

程序中代码公式：

观察矩阵：
即：

H为观察矩阵。

4.2 协方差矩阵

协方差矩阵：

其中，由4.1知A为：

Q为协方差噪声；

Qa为角度误差方差因子，Qg为角速率误差方差因子，这两个认为和元器件参数有关，视为常数。
则：

继续：

(Δt)²太小了，省去，则有：

4.3 卡尔曼增益

卡尔曼增益：

由卡尔曼增益方程计算，有：

故：

得出：

所以：

这里说一下K1，当K1不为0，说明角度误差和角速度误差之间存在相关性，那么角速度的误差ω不应该是一个恒定的常数。

4.4 状态更新方程

状态更新方程：

代码中的公式为：

Kalman_roll_angle为经过加速度计算得到的角度估计值，acc_roll_angle为当前时刻通过加速度分量换算得到的角度值。

4.5 协方差更新方程

协方差更新方程：

展开，有：

进一步计算：

最终得到：

更新Q（ω）值：

5.陀螺仪卡尔曼滤波完整代码

代码部分：
注：这里是加速度和加速度获取后的滤波代码，处理数据为横滚角roll

// 所有变量全部放在main函数中
/*陀螺仪数据*/
short Accel[3];  //加速度分量数组
short Gyro[3];   //角速度分量数组
float Temp;  //温度

float accel_x;	     		//X加速度
float accel_y;	    		//Y加速度
float accel_z;	     		//Z加速度
float gyro_x;						//X角速度
float gyro_y;        				//Y角速度
float gyro_z;		 				//Z角速度

float pitch_raw;  		 		//
float pitch_kalman; 	 		//
float roll_raw;  		  		 	//
float roll_kalman; 		 	//横滚轴卡尔曼	

static float Q_rate; //角速度误差
static float Q_a = 0.003;		//角度误差方差因子，和元器件参数有关，根据情况调整
static float Q_g  = 0.0025;		//角速率误差方差因子，和元器件参数有关，根据情况调整
static float R_matrix = 0.3;		//测量噪声，根据情况调整
static float dt = 0.02;  //20ms
static float K_0,K_1;   //卡尔曼增益
static P_Matrix[2][2] = {{1,0},{0,1}};协方差矩阵初始化为
/* 
   1  0
   0  1
*/

int main(void)
{
	SysTick_Init(); //时钟初始化
	USART_Config();
	i2c_GPIO_Config();
	MPU6050_Init();
	if(MPU6050ReadID() == 0)
	{
		printf("没有检测到6050");
	}
	while(!task_mpu6050_init_finish){};  //检测到6050后进行
	
	while(1)
	if(task_readdata_finish)
	{
		float accx,accy,accz;
		task_readdata_finish = 0;
		/*获取加速度陀螺仪数据*/
		MPU6050ReadAcc(Accel);
		MPU6050ReadGyro(Gyro);
		//赋值
		accel_x = Accel[0];
		accel_y = Accel[1];
		accel_z = Accel[2];
		gyro_x  = Gyro[0];	//x
		gyro_y  = Gyro[1];	//y
		gyro_z  = Gyro[2];	//z.
		/*加速度转换*/
		if(accel_y<32764) 
		{
			accy=accel_y/16384.0;
		}
		else
		{
			accy=1-(accel_y-49152)/16384.0;
		}
		if(accel_z<32764)
		{
			accz=accel_z/16384.0;
		}
		else
		{
			accz=(accel_z-49152)/16384.0;
		}
		roll_raw= atan2(accy,accz) * 180/3.14;
		roll_raw += 180; //将2个±180°区间改为0~360°区间
		/*角速度转换*/
		if(gyro_x<32768) 
		{
			gyro_x=-(gyro_x/16.4);
		}
		if(gyro_x>32768) 
		{
			gyro_x=+(65535-gyro_x)/16.4;
		}
		/*下面利用gyro_x、roll_raw 、dt 做卡尔曼滤波*/
		/*状态空间方程，利用角速度及角速度误差估算当前角度值*/
		roll_kalman = roll_kalman + （gyro_x - Q_rate）*dt;
		/*协方差矩阵计算*/
		P_Matrix[0][0] = P_Matrix[0][0] + (P_Matrix[0][1] + P_Matrix[1][0])*dt + Q_a;
		P_Matrix[0][1] = P_Matrix[0][1] + P_Matrix[1][1]*dt;
		P_Matrix[1][0] = P_Matrix[1][0] + P_Matrix[1][1]*dt;
		P_Matrix[1][1] = P_Matrix[1][1] + Q_g;
		/*卡尔曼增益计算*/
		K_0 = (P_Matrix[0][0]/(P_Matrix[0][0] + R_matrix ));
		K_1 = (P_Matrix[1][0]/(P_Matrix[0][0] + R_matrix ));
		/*状态更新方程，计算出本次最优角度估计值*/
		roll_kalman  = roll_kalman + K_0*(roll_raw  - roll_kalman );
		/*更新协方差矩阵*/
		P_Matrix[0][0] = （1-K_0）*P_Matrix[0][0];
		P_Matrix[0][1] = （1-K_0）*P_Matrix[0][1];
		P_Matrix[1][0] = -K_1*P_Matrix[0][0] + P_Matrix[1][0];
		P_Matrix[1][1] = -K_1*P_Matrix[0][1] + P_Matrix[1][0];
		Q_rate = Q_rate + K_1*(roll_raw  - roll_kalman );
	}
}