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. 回到我们的问题：我们迫切的想知道我们调节了一个参数之后的效果是变好了？还是变坏了？并且要知道大概变好了多少？同样是变好，但是这种比那种好多了！也就是说必须用一个数字来衡量，而不是靠感觉（感觉很不靠谱）。说白了就是需要一个反馈。就好比训练🐕关灯，它做对了你要给它奖励，做错了你要甩它，否则你一直干吼关灯，永远都不会有结果的。

我们使用最简单的评价方法：用方差来表示系统的误差。

误差： h=∑(真实值 - 预测值)^2   ， h值越大说明系统越烂，h值越小说明系统越优秀。这里的真实值 和预测值肯定不是两个值，而是两组，至少是一段时间内的一组值。其中预测值肯定是kalman的输出，那真实值是什么？它是绝对值真是值，也就是说需要把我们的设备放在一个专业的云台上，云台现在的角度是多少，那真实值就是多少。用个减速步进可以很容易的搭建一个：高精度、易控制、高质量的云台。

现在我们就可以观察并且得出，当前状态的最优变化趋势是那个方向（需要改变哪个参数、是增大、还是减小）。

如何防止陷入局部最优解，最简单的方法就是，用不同的值对参数初始化（随机初始化），多收敛几次，哪次最好要那个。这是最简单、最暴力、并且效果很好的方法。当然还有很多的方法可以解决这个问题（搜索：机器学习防止局部最优解之类的即可）。

你可能会有疑问，就用这简陋的破方法就可以？现在深度学习算法的基本思想就是这个，并且效果很好。具体的细节、思路直接搜索深度学习理论即可。

小结（调参步骤）：

a. 采集真实值。配合云台实现。

b. 找到评估函数h。用简单的方差实现。

c.收敛参数。借鉴梯度下降的思路。

d. 实际操作：采集数据集，然后，用电脑进行参数收敛。

假设：采样速度为1000次/秒，采样各种环境下的数据一个小时，共采集三种数据=>真实角度值、加速度计值、陀螺仪值。那么总的数据量=1000次/秒*3种*3600秒=1千万个数据（40兆大小左右）。在深度学习界那么大点数据就是小弟，使用电脑进行参数收敛，会非常的快。

3. 使用这种方法后你会得到非常理想的参数。如果仅仅使用这样的技俩，想干翻大疆，那是不可能。因为我们才只会很简单的调参数，还不会优化、创造算法。

2. 初始化

只有X，P矩阵我们需要关心初始化值。但是因为系统可以快速收敛，所以这里直接使用0初始化就可以。

如果实在想初始化，以加快收敛速度，可以在开始的时候，令X等于当前系统的测量值，即：X[0]=inAngle,X[1]=0。

P矩阵关系稍复杂，并且收敛速度更快，确实没有初始化的必要。

六。 核心理论的具体分析

1. 这里可能有很多人有疑问：

为什么要使用这个公式来更新卡尔曼增益 ，KG=P*H'/(H*P*H+R)，是怎么得出来的这个公式？

 其中的H=[1,0] 又是为什么？H可以=[0, 0.666]吗？

答：这个就比较难搞了。因为需要更多的专业背景知识，例如X=A*X+B*U+W; Y=HX+V 就是线性系统的基本描述。X=X+K*(Z-X), 其中Z-X相当于是误差，那K的意义就明显了，K越大表示对测量值的信任度越高。K值的更新就是根据数据推算，计算出一个尽量使后验协方差最小的值，中间经过一系列的推导最终得到KG=P*H'/(H*P*H+R)。P的更新同理。这里就不过多的讨论了，本篇幅的主要目的意在从公式推出算法、并了解怎么调参。本文最多只是入门导论。后面我会继续更新关于理论的推导过程。

2.算法的优化 

优化的方向有很多，这里大概说一下，大家可以自行查阅。

1. 初始化优化，可以更快速收敛。（很多地方提到，也确实有效，但是很多场景意义不大）。

2. 参数预估，手动干预参数。例如：你知道应用场景噪声会改变，你就手动更变为适应本场景的参数、或进行一定的修改。

3. 延伸到非线性系统。对于非线性系统，要做特殊的改造。因为实际的应用场景非线性的情况很多。【An Introduction to the Kalman Filter】这文章里面有提到（本文底部有网址链接）。

3. 延申讨论

怎么实现简单的、1维的kalman滤波。

float kalman(float inData)	
{
    static float P = 0;
    static float X = 0;
    static float K = 0;
    const float Q = 0.001;  // 过程噪声协方差
    const float R = 0.001;  // 测量噪声协方差
    
    P += Q;	
    K = P/(P + R);
    X += K*(inData - X);
    P = (1‐K)*P;

    return X;
}

可以毫不夸张的说，在部分应用场景，这种简单的一维kalman滤波效果，比中值滤波、高斯平滑的效果好太多。

 

七。收尾

到此，本文就阐述完毕了，如有发现问题或疑问，欢迎指点😀。谢谢合作🙂！

最后编辑：时间 2020年2月13日0点44分

 

参考连接、资料：

1. kalman1960,最初的版本（难懂），【A New Approach to Linear Filtering  and Prediction Problems】。

https://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

2. 【An Introduction to the Kalman Filter】，好理解，专业，逻辑清晰。有扩展非线性的介绍。

http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

3. kalman滤波器的各种资源汇总。搜索：The Kalman Filter  关键字可以找到。

http://www.cs.unc.edu/~welch/kalman/index.html#Anchor-21683 

http://www.cs.unc.edu/~welch/kalman/

4. 比较亲切的介绍，【A practical approach to Kalman filter and how to implement it】，作者有发布代码到github上。

文章：http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

代码：https://github.com/TKJElectronics/KalmanFilter

5. 靠谱、权威的实例代码。

opencv或matlab目录下搜索kalman的文件，我看了，很好懂的。

6. 吴恩达机器学习理论介绍。看机器学习视频即可。 

https://www.coursera.org/

7. 简单的梯度下降算法实现。找到理论说明的文档部分，其中有梯度下降纤细的介绍与代码。

https://tensorflow.google.cn/