直立平衡车的姿态测量卡尔曼滤波算法原理与应用（附代码及调试截图）

        鄙人最近测量调试直立平衡车的姿态角度时，用到了卡尔曼滤波算法。本着知其然还需知其所以然的学习精神，在网上阅览了很多关于滤波原理及算法应用的文章，加上自己的调试经验，有了一点小小的心得，现在分享给大家。疑惑不当之处，欢迎讨论批评。首发于CSDN：http://blog.csdn.net/qq_32666555。转载请注明作者及出处，谢谢！

        首先介绍我的方案背景。我用了惯性测量组合元件（Inertial Measurement Uint，IMU），两轴ENC-03MB陀螺仪及两轴MMA7361加速度计，其中一轴陀螺仪应用在方向测量上，所以不在讨论范围内，测量零点等拓展内容也不在本次讨论范围内。而另一轴陀螺仪测得的角速度作为卡尔曼滤波函数的一个输入变量，两轴加速度计测得的角度之差乘上角度量比例作为卡尔曼滤波函数的另一输入变量。

        下面，我们以“是什么，为何用，怎么用”的顺序来介绍直立平衡车的姿态测量滤波算法原理与应用。因此，先介绍滤波算法的原理。在介绍卡尔曼滤波之前，我们可以先以互补滤波作为敲门石。而介绍互补滤波，就不得不提大名鼎鼎的MIT经典PPT《The Balance Filter》。鄙人亲自翻译了这个牛文，在此附上链接：

        《The Balance Filter》互补滤波器--MIT著名牛文翻译（上）

        《The Balance Filter》互补滤波器--MIT著名牛文翻译（下）

        简单浏览这篇文章，在初步了解了两种传感器工作原理、权重分配、误差漂移、互补滤波等后，我们理解卡尔曼滤波会简单些。

        卡尔曼滤波，于我理解，是只需要k-1时刻两参数的协方差估算出k时刻最优解，并算出k时刻协方差进行递归，算出k+1，k+2…时刻的最优解。因此它是个快速而智慧的算法。知乎上有很多通俗科学的解释，我贴出一个问题链接，里面那些高赞同回答个人觉得都不错：https://www.zhihu.com/question/23971601

        那为何用卡尔曼滤波算法呢？

        首先解释为什么用2种传感器。陀螺仪具精确但有零点漂移特性，其测量误差会随着时间的累加而不断的累积，从而影响测量精度。因此，短时间测量应信任陀螺仪。加速度计一直受到平衡车振动的影响，混叠额外的高频振动量干扰，但是漂移小。因此，长时间测量应信任加速度计。所以单一的传感器测量难以得到精确的姿态角度。需采用多传感器信号融合的方法，来获得准确的姿态角度量。

        而卡尔曼滤波，可以设置分配信任权重，解决陀螺仪零漂、滤除加速度计高频振动，递归自学习，滤波平滑、跟随快。虽然算法较复杂，占用较多线程时间，但是我们选用的freescale K60处理器完全可以承担这样的计算，所以决定使用该滤波算法。

最后，就是卡尔曼滤波算法的应用了。Show you the code.

float Kal_Gyro;
float gyro;
int16_t mmax, mmaz;
float angle, angleSpeed;
float Angle_Kalman;
int16_t Pre_Angle_Kalman;
float angleSpeedIntegral;

float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/**
 * @brief  卡尔曼滤波, 输入带有噪声的参数组, 返回可靠的参数组, 此处用于获得变化较稳定的角度值
 * @param[in]  Accel 由加速度计得到的角度参量
 * \param[in]  Gyro 由陀螺仪得到的角速度参量
 * @retval 稳定可靠的角度值, 用于直立控制
 */

static float KalmanFilter(float Accel,float Gyro)		
{
    static float Angle = 0, Gyro_y = 0;
	Angle+=(Gyro - Q_bias) * dt; //先验估计

	Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

	Pdot[1] = -PP[1][1];
	Pdot[2] = -PP[1][1];
	Pdot[3]= Q_gyro;
	
	PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
	PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;
		
	Angle_err = Accel - Angle;	//zk-先验估计
	
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	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;
		
	Angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	Gyro_y   = Gyro - Q_bias;	 //输出值(后验估计)的微分=角速度
	return Angle;
}

因为我是按The balance filter里的坐标系修改的，所以

@param[in] angle_kal 由加速度计得到的角度参量，是两轴加速度计测得的角度之差乘上角度量比例再乘初始所占权重；

\param[in] angle_speed_kal 由陀螺仪得到的角速度参量，是陀螺仪测得的角速度的相反值；

@retval 稳定可靠的角度值, 用于直立控制，是送入角度调节中的当前量。

而卡尔曼滤波的5个方程 

X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)//先验估计
P(k|k-1)=A P(k-1|k-1) A’+Q ………(2)//协方差矩阵的预测
Kg(k)= P(k|k-1) H’ / (HP(k|k-1) H’ + R) ………(3)//计算卡尔曼增益
X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ………(4)//通过卡尔曼增益进行修正
P(k|k)=（I-Kg(k) H）P(k|k-1) ………(5)//更新协方差阵


（1）根据角度微分等于时间的微分乘以角速度，k时刻角度可近似认为是k-1时刻的角度值加上k-1时刻陀螺仪测得的角加速度值乘以时间，同时要减去陀螺仪的静态漂移。

angle_kalman += (angle_speed_kal - Kal_Gyro_Zero) * Kalman_Sample_Time;// Kalman_Sample_Time 是卡尔曼滤波采样时间参量，应调节到使滤波平滑，跟随快滞后少，此处为0.0055.

结合其余定义式，写成矩阵形式

对应卡尔曼滤波第一个方程
X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)//先验估计

（2）预测方差阵的预测值，噪声就是数据的方差值

#define Q_angle     0.001 //角度噪声
#define Q_gyro      0.0005//漂移噪声



Pdot[0] = Q_angle - Pk[0][1] - Pk[1][0];

Pdot[1] = -Pk[1][1];

Pdot[2] = -Pk[1][1];

Pdot[3] = Q_gyro;

Pk[0][0] += Pdot[0] * Kalman_Sample_Time;

Pk[0][1] += Pdot[1] * Kalman_Sample_Time;

Pk[1][0] += Pdot[2] * Kalman_Sample_Time;

Pk[1][1] += Pdot[3] * Kalman_Sample_Time;

对应卡尔曼滤波第二个方程
P(k|k-1)=A P(k-1|k-1) A’+Q ………(2)//协方差矩阵的预测

（3）计算卡尔曼增益K_0，K_1
#define R_angle     0.05  //角度测量噪声值 


angle_err = angle_kal - angle_kalman;

PCt_0 = C_0 * Pk[0][0];

PCt_1 = C_0 * Pk[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

对应卡尔曼滤波第三个方程

Kg(k)= P(k|k-1) H’ / (HP(k|k-1) H’ + R) ………(3)//计算卡尔曼增益

（4）对矩阵Pk进行更新
t_0 = PCt_0;

t_1 = C_0 * Pk[0][1];

Pk[0][0] -= K_0 * t_0;

Pk[0][1] -= K_0 * t_1;

Pk[1][0] -= K_1 * t_0;

Pk[1][1] -= K_1 * t_1;

对应卡尔曼滤波第五个方程

P(k|k)=（I-Kg(k) H）P(k|k-1)………(5)//更新协方差阵

（5）通过卡尔曼增益修正当前的角度、陀螺仪零点，算出当前角速度

angle_kalman += K_0 * angle_err;

Kal_Gyro_Zero += K_1 * angle_err;

Kal_Gyro = angle_speed_kal - Kal_Gyro_Zero;


对应卡尔曼滤波第四个方程

X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ………(4)//通过卡尔曼增益进行修正

最后，附上本人的测试曲线图及实物效果图，谢谢大家！

（2018.1.28更新，上面贴的代码也更新了）

这个是刚能直立时的1.0哈哈，见笑了

这是比赛前一天试车的截图，2.5m/s左右吧。旁边赛道的脚是厦大的同学的。

我们试完车轮到中南大学的大佬，他们最后拿了直立组第一。