kalman姿态解析, 带公式推导, 带注解说明, 带源码_卡尔曼姿态解算

一. 本博文的目标

1. 推导kalman姿态解析的公式。
2. 根据公式直接导出代码，保证代码可用(不看理论的情况下：也能轻松使用)，并且代码和公式完全匹配。
3. 解释部分参数的含义、调试心得。

二. 概要

1. 本文存在的意义：

现在已近用很多的关于kalman姿态解析的文章、论文了，为什么我还要继续？

答：很多文章只讨论了公式、理论，却没用代码。有些有代码（因为极度优化）不知道是什么鬼，其中是否有错也不得而知。还有一些代码和他的理论不能匹配的。当然还有很多优秀的英文版本，但是不晓得孰优劣。再者就是因为kalman的错误容忍能力极强，你的代码在部分出错的情况下，算法也能得到相对良好的结果。

本文保证：本文所述理论和代码严格的对应。至少理论部分不会自相矛盾。

2. 代码使用简介:

/*
inAngle:        输入测量的角度值(来源于加速度计)
inAngleSpeed：  输入测量的角速度值（来源于陀螺仪）
return：        返回预测的角度值
*/
float kalman(float inAngle,float inAngleSpeed);

三. 理论推导

1. 其实就去去5个公式而已，先看一些全貌，很简单的加减乘除。（先别细看）

X=A*X+B*U；

P=A*P*A'+Q ；

K=P*H'/(H*P*H'+R)；

X=X+K*(Z-H*X)；

P=(I-K*H)*P；

2. 预备知识：

1).  A*B 表示A叉乘B

2).  A' 表示A的转置矩阵

3).  A=[1,2]         表示1*2的行矩阵；

      A=[1;2]          表示2*1的列矩阵；

      A=[1,2;3,4]   表示2*2的矩阵

3. 开始推导（先不用管公式为什么是这样）

1).  X=A*X+B*U；  

X=[angle;angleSpeedBias]  预测矩阵

这是一个2*1的列矩阵，其实就是系统的预测的输出缓存，第一个代表角度，第二个代表角速度的静态偏差值（输出预测角速度=测量角速度-静态偏差）。

A=[1,-TS;0,1]  转移矩阵

一个固定值的矩阵，其中TS是采样时间（常数）。

B=[TS;0]  控制矩阵

一个固定值的矩阵

U=[inAngleSpeed] 输入

是系统输入的角速度值

2）. P=A*P*A'+Q ；

P：协方差矩阵，是一个2*2的矩阵。是对当前系统噪声的估计（分先验和后验）。

A：前面说过了，一个固定值矩阵。

Q： [Q_a, 0;  0, Q_b]*TS

这是噪声的协方差矩阵，2*2的矩阵。其中q_a表示角度的方差，q_b表示角速度偏差的方差。说通俗点：这两个值表示对两种传感器的置信度，也就是我们要调试的参数之一。一般来讲陀螺仪、加速度计的置信度不一样，所以q_a一般不等于q_b。

3).  K=P*H'/(H*P*H'+R)；

K：卡尔曼增益矩阵，2*1的列矩阵，系统会不断的更新这个值，用于预测输出。

P：前面的。

H：[1,0]   一个1*2的固定值行矩阵。

R：R_m   一个常量，表示整个测量系统的方差，是要调试的参数之一。

4). X=X+K*(Z-H*X)；

X：这里就是更新输出，outAngle=X[0]; outAngleSpeed = inAngleSpeed - X[1];

K：前面的。

H：前面的。

Z：[inAngle]  输入角度

5). P=(I-K*H)*P；

P: 跟新协方差估计

I： [1, 0;  0, 1]    是一个2*2的单位矩阵

K：前面的。

H：前面的。

汇总下遇到的所有矩阵(或常数)：

输入：

U=inAngleSpeed;

Z=inAngle;

输出：

X[0] : outAngle;

X[2]: outAngleSpeedBias

常量：

TS：采样时间配置，是多少就输入多少。

Q_a,Q_b,R_m：角度计、角速度计、系统测量误差估计值。属于需要调试的三个参数。

A=[1,-TS;0,1]  

B=[TS;0]

Q=[Q_a, 0;  0, Q_b]*TS

H=[1,0] 

R=R_m

I= [1, 0;  0, 1] 

到此为止理论的介绍完毕！后面再讲解参数、公式的具体含义！

 

四。导出代码

直接根据5个公式得出最暴力的结果：

*如果你使用的是matlab,  那就爽爆了，直接就可以写成如下的代码（当然需要初始化A,B,H,I等矩阵），因为matlab是直接支持矩阵的各种运算的，所以代码非常的简单。

% matlab 模拟代码
function kalman(U,Z,config){
    // 初始化、转换输入、参数配置
    ...
    // 5大核心步骤
    X=A*X+B*U;
    P=A*P*A'+Q;
    K=P*H'/(H*P*H'+R);
    X=X+K*(Z-H*X);
    P=(I-K*H)*P;
    // 返回
    return X;
}

现在用纯C实现如下。主要就是手动实现：矩阵叉乘、加减、转置。 没有什么难度，保证不出错就可以。

当然，如果直接只要使用这样的C代码是没有任何问题的，但是就是效率有点底下。接下来的一步就很明显了，那就是化简、约分、矩阵简化。

// 原始版本
function original(U,Z)
{
    // 变量定义
    A=[ [1,-TS],
        [0,  1] ];
    B=[ [TS],
        [0]] ;
    Q=[ [Q_angle,0],
        [0,Q_angleSpeedBias] ];
    H=[ 1,0 ];
    R=[ R_measure ];  
    I=[ [1,0],
        [0,1] ];
    U=[ inAngleSpeed ];
    Z=[ inAngle ];
    
    X.size=[2,1];
    P.size=[2,2];
    KG.size=[2,1];
 
    //第一步 X=A*X+B*U;
    X[0]=A[0][0]*X[0]+A[0][1]*X[1]+B[0]*U    
    X[1]=A[1][0]*X[0]+A[1][1]*X[1]+B[1]*U
 
    //第二步 P=A*P*A'+Q 
    P[0][0]= (A[0][0]*P[0][0]+A[0][1]*P[1][0])*A[0][0]+(A[0][0]*P[0][1]+A[0][1]*P[1][1])*A[0][1]+Q[0][0]
    P[0][1]= (A[0][0]*P[0][0]+A[0][1]*P[1][0])*A[1][0]+(A[0][0]*P[0][1]+A[0][1]*P[1][1])*A[1][1]+Q[0][1]
    P[1][0]= (A[1][0]*P[0][0]+A[1][1]*P[1][0])*A[0][0]+(A[1][0]*P[0][1]+A[1][1]*P[1][1])*A[0][1]+Q[1][0]
    P[1][1]= (A[1][0]*P[0][0]+A[1][1]*P[1][0])*A[1][0]+(A[1][0]*P[0][1]+A[1][1]*P[1][1])*A[1][1]+Q[1][1]
 
    //第三步 K=P*H'/(H*P*H'+R)
    // temp = H*P*H' + R
    temp = (H[0]*P[0][0] + H[1]*P[1][0])*H[0] + (H[0]*P[0][1] + H[1]*P[1][1])*H[1] + R
    // K = P * H'/ temp
    KG[0] = (P[0][0]*H[0]+P[0][1]*H[1]) / temp
    KG[1] = (P[1][0]*H[0]+P[1][1]*H[1]) / temp
 
    //第四步 X=X+KG(Z-H*X)
    X[0]=X[0]+(Z-(H[0]*X[0]+H[1]*X[1]))*KG[0]
    x[1]=X[1]+(Z-(H[0]*X[0]+H[1]*X[1]))*KG[1]
 
    //第五步 P=(I-KG*H)*P
    P[0][0]= (I[0][0]-KG[0]*H[0])*P[0][0]+(I[0][1]-KG[0]*H[1])*P[1][0]
    P[0][1]= (I[0][0]-KG[0]*H[0])*P[0][1]+(I[0][1]-KG[0]*H[1])*P[1][1]
    P[1][0]= (I[1][0]-KG[1]*H[0])*P[0][0]+(I[1][1]-KG[1]*H[1])*P[1][0]
    P[1][1]= (I[1][0]-KG[1]*H[0])*P[0][1]+(I[1][1]-KG[1]*H[1])*P[1][1]
 
    // 返回
    return X;
}

可以看到，A,B,H,I,Q等矩阵中有很多 等于 1或0 的值，把对应的进行约去即可。保证不出错即可。过程很简单。

现在展示第一个公式的约分步骤，然后再以此类推。可以看到没有任何的难度。例如：

公式 1 简化过程： X=A*X+B*U;
X[0]=A[0][0]*X[0]+A[0][1]*X[1]+B[0]*U;            
X[1]=A[1][0]*X[0]+A[1][1]*X[1]+B[1]*U; 
其中：
A=[1, -TS; 0, 1];  B=[TS; 0];  U=inAngleSpeed;
 
第一步:
X[0]=1*X[0]-TS*X[1]+TS*U;            
X[1]=0*X[0]+1*X[1]+0*U; 
 
第二步：
X[0]=X[0]-TS*X[1]+TS*inAngleSpeed;            
X[1]=X[1];                           // 相当于什么都没做，直接不执行即可
 
第三步：
X[0]+=(inAngleSpeed-X[1])*TS;

其他所有公式的简化方法、步骤同上，这里就直接显示结果，如下。网上流传的几种代码和这个代码相似，但这里你就可以很轻松的看出，每一行代码的意义、来源。这样思路就很清晰了很多。

// 初步简化版本
float kalman(float inAngle, float inAngleSpeed)
{
    // 缓存
    static float X[2]={0.0f, 0.0f};
    static float P[2][2]={{0.0f, 0.0f}, {0.0f, 0.0f}};
    static float KG[2]={0.0f, 0.0f};
    // 参数配置
    const float TS = 0.001;                  // 采样时间设置
    const float Q_angle = 0.001;             // 角度计(加速度计)噪声估计
    const float Q_angleSpeedBias = 0.003;    // 角速度计(陀螺仪)噪声估计
    const float R_measure = 0.03;            // 系统误差估计
 
    // 输入转换       X=A*X+B*U
    X[0]+=(inAngleSpeed-X[1])*TS;
 
    // 协方差估计     P=A*P*A'+Q  
    P[0][0]+=TS*(Q_angle-P[1][0]-P[0][1]+TS*P[1][1]);
    P[0][1]-=TS*P[1][1];
    P[1][0]-=TS*P[1][1];
    P[1][1]+=TS*Q_angleSpeedBias;
 
    // 卡尔曼增益计算  K=P*H'/(H*P*H'+R)
    KG[0]=P[0][0]/(P[0][0] + R_measure);
    KG[1]=P[1][0]/(P[0][0] + R_measure);
 
    // 预测输出        X=X+KG(Z-H*X)
    X[0]+=(inAngle-X[0])*KG[0];
    x[1]+=(inAngle-X[0])*KG[1];
 
    // 协方差更新      P=(I-KG*H)*P
    P[0][0]-= KG[0]*P[0][0];
    P[0][1]-= KG[0]*P[0][1];
    P[1][0]-= KG[1]*P[0][0];
    P[1][1]-= KG[1]*P[0][1];
 
    // 输出
    float outAngle = X[0];
    float outAngleSpeed = inAngleSpeed - x[1];
    return outAngle;
}

其实上面的代码从效率的角度来讲还是可以再优化的，思路：2维=>1维，1维=>变量，去重复计算。具体的优化结果如下。

如果你不关心理论，想使用，直接使用下面的代码即可。

/*
强力加速版本
inAngle：       输入 测量角度（单位：度）
inAngleSpeed：  输入 测量角速度（单位：度每秒）
return：        输出 预测角度（单位：度）
*/
float kalmanFast(float inAngle, float inAngleSpeed)
{
    // 长期缓存
    static float X0=0.0f,X1=0.0f;
    static float P00=0.0f, P01=0.0f,P10=0.0f, P11=0.0f;
    // 临时缓存
    float KG0=0.0f, KG1=0.0f;
    float TSMulP11=0.0f;                     // TSMulP11 = TS*P[1][1]
    float S=0.0f;                            // S = P[0][0] + R_measure
    float E=0.0f;                            // E = angular_m-X[0]
    // 参数配置
    const float TS = 0.001;                         // 采样时间设置（单位：秒）；注：这几个数据的单位只要统一即可。
    const float Q_angle = 0.001;                    // 角度计(加速度计)噪声估计
    const float Q_angleSpeedBias = 0.003;           // 角速度计(陀螺仪)噪声估计
    const float R_measure = 0.03;                   // 系统误差估计
 
    // 输入转换       X=A*X+B*U
    X0 += (inAngleSpeed - X1)*TS;
 
    // 协方差估计     P=A*P*A'+Q  
    TSMulP11 = TS*P11;
    P00 += TS*(Q_angle - P10 - P01 + TSMulP11);
    P01 -= TSMulP11;
    P10 -= TSMulP11;
    P11 += TS*Q_angleSpeedBias;
 
    // 卡尔曼增益计算  KG=P*H'/(H*P*H'+R)
    S = P00 + R_measure;
    KG0 = P00/S;
    KG1 = P10/S;
 
    // 预测输出        X=X+KG(Z-H*X)
    E = inAngle - X0;
    X0 += E*KG0;
    X1 += E*KG1;
 
    // 协方差更新      P=(I-KG*H)*P
    P00 -= KG0*P00;
    P01 -= KG0*P01;
    P10 -= KG1*P00;
    P11 -= KG1*P01;
 
    // 输出
    return X0;
}

五。 调试参数

1. 参数

需要配置的参数就有4个：TS，Q_angle，Q_angleSpeedBias，R_measure；

TS：

指的是系统的采样时间，是多少就填多少，无所谓调参数。

Q_angle：角度_过程噪声协方差

表示对加速度计的置信度，值设置的越大表示对加速度计的信任度越高。如果这个值远大于其他值，那么输出结果会和使用加速度计换算出来的角度值高度重合。

Q_angleSpeedBias：静差_过程噪声协方差

表示对陀螺仪的置信度，值越大表示对陀螺仪的信任度越高。如果这个值远大于其他值，则输出的角度值理论上等于角速度值的积分。（因为陀螺仪有静差，这里又进行积分，所以会又严重的漂移，所以说理论上）。

R_measure：测量噪声协方差

表示对我们整系统的置信度，值越大表示对我们算法的信任度越高。如果值过大，预测值会异常的平滑，但是严重的反应迟钝。如果值过小，会出现输出的值基本等于测量的值，相当于没有滤波。

总结：

是不是豁然开朗？如果我看到输出值和加速度计的值高度吻合，并且毛刺大，就减小Q_angle，如果系统零飘严重我就减小Q_angleSpeedBias的值，如果各种调参都没有看到滤波效果我就增大R_measure试试。

那你就太天真了，那还要【童子手】调参干什么。因为很多时候根本就不知道要调节那个参数。例如：输出反应稍慢，且有微小过充，你觉得是增大Q_angle，还是减小Q_angleSpeedBias，还是减小R_measure。感觉理论上都可以，但是要先改变那个呢？改多少呢？

那调参就是不可能的吗？不！kalman的适应能力超强，很烂的参数也会有不错的效果，基本上按照上面的调试理论+感觉，试个几十次后就会得到比较理想的结果（至少比直接使用加速度计推算的角度值强太多）。如果想做个简单的可以飞的四轴飞行器、简单的平衡车，那么你基本是个几百次就可以达到非常良好的结果。

当然如果你有干翻大疆的想法，那就有得搞了。你必须有更为科学的调试方法才能调试出比较好的结果。这里说个大致的思路。

1.  灵感：当今世界效果特好、参数又巨多、并且不知道其中任何参数的意义的算法是什么？答案就是机器学习算法之类的，例如神经网络算法，无论是多层感知网络、卷积神经网络、残差网络还是其他。它的参数基本都是以万为单位的，并且根本就不知道参数的含义是什么，但很多时候却有非常好的效果。它怎么做到的呢？

它使用的最原始的收敛参数方法就是：梯度下降算法。举个例子，你在一个没有gps、大雾弥漫可视距离10米、没有路的山上，你怎么下山。你环顾四周找到最陡且向下的方向走10米，然后在以此类推。走着走着你就可能下山了。当然你可能只走到了一个凹坑里无法自拔，因为四周都比你现在的位置高，这叫做局部最优解。

关键步骤：要知道现在每个方向的变化趋势，向好趋势的方向前进一步，继续观察，重复上述步骤。

2. 回到我们的问题：我们迫切的想知道我们调节了一个参数之后的效果是变好了？还是变坏了？并且要知道大概变好了多少？同样是变好，但是这种比那种好多了！也就是说必须用一个数字来衡量，而不是靠感觉（感觉很不靠谱）。说白了就是需要一个反馈。就好比训练