浅谈卡尔曼滤波器

一． 状态观测器

1. 当只知道原系统的输入矩阵U|k和输出矩阵Y|k，但是无法得知某些系统状态X|k时，可以采用状态观测器的方法。
2. 状态观测器即是将原系统模拟成一个观测器系统，观测器的输入矩阵为原系统的 U|k。若观测器完全复现了原系统，即观测器输出输出矩阵Ŷ|k将与原系统相等，这时即可认为原系统待观测状态矩阵X|k与观测器状态矩阵X^|k相等；然而事实上因为存在各类噪声(如过程噪声W|k、测量噪声V|k等)，观测器总是不能完全拟合原系统，因此引入了负反馈调节。
3. 负反馈调节的反馈值是原系统与观测器输出值之差，反馈增益为K；当存在输出偏差时，反馈系统就会使这个偏差按反馈增益作用于观测系统，从而消除原系统中的各种噪声，使偏差逐渐趋近于0。
   
4. 数学分析
   
   因此当A-KC<0时，系统状态值偏差会趋近于0，从而可以通过观测器系统得到待观测的状态值X|k。所以当反馈器增益K选取合理，就可以使得A-KC<0。

二． 卡尔曼滤波器——特殊的状态观测器

1. 卡尔曼5条公式
   

2. 预测阶段：kalman先是根据这一时刻的输入值、上一时刻的估计值得出这一时刻的预测值。接着是根据上一时刻的P|k和过程误差，得出预测值与真实值误差的协方差P|k-（即预测值与真实值误差的方差）；P|k是估计值与真实值误差的协方差，Q是假设过程误差W|k满足高斯分布(0,Q)。

3. 更新阶段：K是kalman增益，它是通过使P|k趋近于最小而推导出来的，也就是说K数值的更新会让估计值趋近于真实值，R是假设测量误差V|k满足高斯分布(0，R)。接着是更新估计值，它由当前观测值和预测值组成，两者的比例由kalman增益控制。最后根据预测值和估计值的状况更新P|k。

4. 卡尔曼滤波器对比起一般的状态观测器有三个特点：一是根据上一估计值和输入值先做一次前置预测（该前置预测存在误差）；二是原系统和观测器输出误差的反馈增益K为自适应参数（该参数的调整会使得状态估计值趋近于真实状态值），作为输出偏差的增益与预测值一同得出最优估计值；三是反馈回路的最终反馈值为上一时刻的估计值（当上一时刻估计值与真实值仍然存在偏差时，就会修正当前时刻的kalman增益，得出趋近于真实值的估计值）。

三． 应用举例——六轴传感器

1. 前期分析：

a). 三轴加速度计在静止时，通过反正切计算，可精确得到Pitch和Roll角度，但有如下限制：

• 在运动时数值波动大，即为高频噪声；
• 因为加速度计的工作原理以及反正切计算的原因，在Pitch和Roll转过临界值时（0-90-0-(-90)-0），角速度计累加获取的角度会与其存在偏差，此时kalman需要一定的时间去收敛并且对后续运算有影响，所以需要对角速度计获取的角度做临界处理。

根据atan2公式，可知存在临界值

arctan公式图像

b). 三轴角速度计通过积分可以得到三轴角度，但是因为零飘和温飘的存在，使得数值会偏移，即存在低频噪声。

c). 通过上诉分析，我们再根据kalman第四条公式，可将输出值Y|k设为角加速度得到的角度，当静止时数值稳定，估计的状态值会更倾向于选择Y|k；但是当动态时，kalman增益K必然会降低，使得估计的状态值会不倾向于选择Y|k。那么此时选择哪个数值呢？答案是角速度积分得出的角度，因此我们可以将零飘处理后的角速度值作为输入值，那么预测的状态值就可以根据当前角速度积分和上一时刻估计角度值得出了。

2. 模型建立：

• 按照上述分析，中间待观测的状态量定为真实角度值即可，但是本人按照此模型去测试，发现无法兼顾真实角度值的响应速度和抗高频噪声性能，如下图是属于折中的情况(红色为角速度积分，黄色为加速度反正切，紫色为kalman后的角度值)，但整体效果不好。后来在网上发现有人将待观测状态矩阵设为2维，即将中间待观测的状态量定为 [真实的角度；角速度偏差]；

• 则此时状态转移矩阵A为2x2矩阵，控制输入矩阵B为2x1矩阵，如下图，因为当前预测角度=上一时刻角度估计值 + (当前角速度－上一时刻角速度偏差估计值)x步长， 而当前预测角速度偏差=上一时刻角速度偏差估计值，所以A为 [1，-△t；0，1] ，B为 [△t；0] 。

• 同理可知状态观测矩阵C为1x2矩阵，且根据系统输出值和状态值的关系，可知C为 [1，0] 。

• 同理根据kalman第二条公式，可知P|k-、P|k和过程误差方差矩阵Q都为2x2；P|k需要为P|k-提供初始值，这里将P|k设为单位矩阵，将P|k-设为零矩阵；根据状态矩阵可知Q应为角度过程误差的方差和角速度偏差过程误差的方差，可知这两个方差之间是独立的，所以设为对角矩阵。

• 同理根据kalman第三条公式，可知K为2x1矩阵，将其初始都设为0；R为1x1矩阵，将其设为角度测量误差的方差。

3.参数调整

• 根据kalman增益公式我们可以做如下推导
• 表明当我们前期分析时若信任观测值，认为其噪声的含量不大，则可以适当减小测量噪声方差；在此模型中因为加速度动态时存在大量高频噪声，所以可以适当增大R，但若太大会导致静态时收敛到真实值的速度变慢，即会存在稳态误差，甚至存在数值漂移现象(等同于只需要预测值)。
• 同理先验误差协方差受到过程方差的影响，若我们信任建立的状态方程以及输入值，则可以减小过程噪声方差；在此模型中因为角速度静态时存在低频噪声，因此我们可以适当调整Q(总体数值偏小)；当Q过大时，此时会导致响应速度变慢同时抗高频噪声能力减弱；当Q过小时，会导致存在稳态误差以及数值漂移现象。

4. 代码编写

• 根据前面的分析，定义如下数据结构

typedef struct
{
    float A[2][2];  //状态转移矩阵
    float B[2];     //控制输入矩阵
    float C[2];     //观测矩阵

    float Q_Measure_VAR[2][2];  //测量噪声的方差
    float R_Process_VAR;        //过程噪声的方差
    float K[2];                 //卡尔曼增益
    float P_Predict[2][2];      //先验值协方差
    float P_Evaluate[2][2];     //后验值协方差

    float X_State_Predict[2];   //先验状态值
    float X_State_Evaluate[2];  //后验状态值
    float Y_Observe_Out;        //观测输出值
    float U_In;                 //输入值
	
	float t;										//步长
}kalman_SixAxis_t;

• 根据kalman公式，编写如下处理过程

void kalman_operate(kalman_SixAxis_t *kalman)
{
	/*对状态矩阵进行预测，得到先验值*/
	kalman->X_State_Predict[0] = kalman->X_State_Evaluate[0] + ( kalman->U_In - kalman->X_State_Evaluate[1]) * kalman->t;
	kalman->X_State_Predict[1] = kalman->X_State_Evaluate[1];
	
	/*得出先验值与真实值的误差协方差矩阵*/
	kalman->P_Predict[0][0] = kalman->P_Evaluate[0][0] + \
		( kalman->P_Evaluate[1][1] * kalman->t - kalman->P_Evaluate[1][0] - kalman->P_Evaluate[0][1] ) * kalman->t + kalman->Q_Measure_VAR[0][0];
	kalman->P_Predict[0][1] = kalman->P_Evaluate[0][1] - kalman->P_Evaluate[1][1] * kalman->t;
	kalman->P_Predict[1][0] = kalman->P_Evaluate[1][0] - kalman->P_Evaluate[1][1] * kalman->t;
	kalman->P_Predict[1][1] = kalman->P_Evaluate[1][1] + kalman->Q_Measure_VAR[1][1];
	
  /*更新卡尔曼增益矩阵*/
	kalman->K[0] = kalman->P_Predict[0][0] / ( kalman->P_Predict[0][0] + kalman->R_Process_VAR );
	kalman->K[1] = kalman->P_Predict[1][0] / ( kalman->P_Predict[0][0] + kalman->R_Process_VAR );
	
	/*更新后验值，得出状态矩阵的估计值*/
	kalman->X_State_Evaluate[0] = kalman->X_State_Predict[0] + kalman->K[0] * ( kalman->Y_Observe_Out - kalman->X_State_Predict[0] );
	kalman->X_State_Evaluate[1] = kalman->X_State_Predict[1] + kalman->K[1] * ( kalman->Y_Observe_Out - kalman->X_State_Predict[0] );
	
	/*更新后验估计值与真实值误差的协方差矩阵*/
	kalman->P_Evaluate[0][0] = ( 1 - kalman->K[0] ) * kalman->P_Predict[0][0];
	kalman->P_Evaluate[0][0] = ( 1 - kalman->K[0] ) * kalman->P_Predict[0][1];
	kalman->P_Evaluate[0][0] = kalman->P_Predict[1][0] - kalman->K[1] * kalman->P_Predict[0][0];
	kalman->P_Evaluate[0][0] = kalman->P_Predict[1][1] - kalman->K[1] * kalman->P_Predict[0][1];
}

• 初步测试效果

a). kalman处理后的角度值除了初始时，迭代收敛至稳定过程中会有数值漂移，其余时段只有±0.01的波动；而角速度积分得到的角度值，则一直存在数值偏移。（kalman_P_angle为kalman处理后的角度值，P_angle_sum为角速度积分得来的角度值）

b). kalman处理后的角度响应速度完全可以跟得上角速度计得出的角度（因为这里角速度计存在数值漂移，所以不需要考虑数值误差，只需要考虑响应速度这个性能）

c). 静态时，加速度计得出的角度与kalman得出的角度基本重合，但kalman得出的角度波动更小，加速度计得出角度的波动在±0.1

此图为45°沿x轴自由落下，可以看出kalman处理后的角度可以非常有效地抵抗高频噪声；同时静态时，有很好的收敛速度。

以上是我对卡尔曼滤波器的一点理解，不足之处请在评论区指出。

参考资料：

• https://www.zhihu.com/question/23971601/answer/839664224
• https://zhuanlan.zhihu.com/p/88535121
• https://zhuanlan.zhihu.com/p/48876718
• https://zhuanlan.zhihu.com/p/36374943