卡尔曼滤波器和六轴传感器姿态融合资料整理

卡尔曼滤波器资料整理

• 卡尔曼滤波器实现6轴姿态估计: A practical approach to Kalman filter and how to implement it [重要]
  
  • 网上有翻译版,但质量不太好.例如innovation 翻译为创新,实际应为残差.
  • 原文有一个tilt sensing pdf链接.主要介绍三轴加速度计.对理解板上的坐标系,旋转,旋转顺序,重力向量很有帮助.特别是理解加速度计只能测量 roll-pitch 这一点.
• 卡尔曼滤波的推导(中文)
• 推导2
• 这篇博文比较全面地对调整几个噪声协方差矩阵做实验.理解Kalman滤波的使用
• 小木虫论坛对参数的讨论

卡尔曼笔记有上面几个链接已经相当完善了.这篇博文主要目的是给自己加深印象.下面结合六轴实例(Lauszus的实现)讲讲我对应用卡尔曼做姿态融合的理解.

IMU 模型

6轴IMU 使用卡曼滤波拟合加速度计和陀螺仪原理在第一个链接.
总结几点:

• 加速度计作为观测模型,陀螺仪作为状态变换模型.
• 陀螺仪每次输出的是角速度.(通过惯性力矢量在 xyz 上的投影反映).
• 加速度计也是输出一个矢量投影,但是反映姿态旋转(roll/pitch).顺序可以自己决定.
  
  • 此时传感器姿态代表一个旋转矩阵,这个矩阵也可以代表一个坐标系,一组正交基.既然是旋转矩阵那么roll-pitch 或 pitch-roll 都能得到这个矩阵.
• 解释一下代码里16384.0f 这个数字.在 register map spec. 有提到.这是指该芯片最低有效位对应的量程.这个系列的芯片输出都是16位.每种芯片的量程都映射到0-65535这个范围.量程越小的映射(/LSB sensitivity)回来精度越高.充分利用寄存器每一位,非常巧妙的设计.
  

欧拉角和 yaw/pitch/roll 关系

后一种被称为Tait–Bryan angles.是欧拉角的一种.旋转顺序可以是(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z).而欧拉角还有一种这样的: y-x-y. 只绕两个轴完成旋转.

一些问题

Q1:为什么官方例程又是 getmotion6,又是 dmpQuaternion 的.有什么区别?

Q2:getmotion6 三轴算出来的旋转量是什么?旋转顺序?

Q3:The key .如何从加速度向量推导欧拉角.(加速度计只能推出两个)

以上问题是看 arduino 的 MPU6050官方例程时想到的.研究源码发现 getmotion6 和dmpQuaternion都是直接从芯片获得的数据!dmpQuaternion是芯片本身应用某种滤波器的结果,不是卡尔曼滤波器.

Q: P 到底是啥,怎么起作用?为什么叫协方差矩阵却从来没有算过协方差?

回答 Q3

可以结合这个链接看看.
把旋转过程理解为基的旋转会更容易: 想象重力矢量为(0,0,-1),当前坐标系 z 轴和重力反方向; 当坐标系绕 x 轴旋转为 roll,再绕 z 轴转为 pitch.得到新的坐标系.此时的重力矢量在这个坐标系下的值就是加速度计在 xyz 轴读出来的值.用这些值可以反推出 roll ,pitch 的角度大小.
Tilt Sensing Using a Three-Axis Accelerometer.pdf 推导了从重力矢量到 roll-pitch 角度的过程.

卡尔曼笔记

\< A practical approach to Kalman filter and how to implement it> 的笔记.

各个符号的含义:

symbol implication example xk k时刻的状态向量 角度和角速度偏差xk=[θθ˙b]k F 状态转移矩阵，和具体的线性系统相关 uk K时刻外界对系统的作用,[θ˙k,0]T θ 角度 θb˙ 角速度偏差 θ˙ 角速度 B 输入控制矩阵，外界的影响如何转化为对状态的影响 P 误差矩阵 , 预测值和真实值的误差的协方差方程.\n 因为当前状态向量有两个变量(角度和角速度),\n 所以此为2x2矩阵.记住主对角线代表两个变量的方差,副对角线代表误差相关程度 Q 预测噪声协方差矩阵 R 测量噪声协方差矩阵 H 观测矩阵 Kk K时刻的kalman增益 zk K时刻的观测值

一般带\^的为估计值.下面很多公式复制来的,有时没有加上也是估计值.懒得加了.
卡尔曼五个方程中,有些是比较好理解,可以直接建模的。有些则是推导而来的。

Step1:

陀螺仪计算出来角速度是控制变量输入,加速度计是观测值.使用差分方程表示系统,对状态向量建模:
[θθ˙b]k|k−1=[θk−1+Δt(θ˙−θ˙b)θ˙b]
写成矩阵形式就能写出KF的第一个式子.

xk=Fxk−1+Buk+wk

[θθ˙b]k|k−1=[10−Δt1][θθ˙b]k−1|k−1+[Δt0]θ˙k

关键在于建模要准确.模型预测要尽可能贴近真实值.如果建模不准 , 可能整体滤波结果落后与真实值.见链接的实验.角速度偏差θb˙后面讨论.

Step2

Pk|k−1=FPk−1|k−1FT+Qk

然后第二方程,误差协方差矩阵P 的预测方程是推导出来的.但是 lauszus 只说这是定义.参考链接重写一下这个推导过程:
假设真实值为 x.用我们的模型表示,则零输入下为:

xk=[θ,0]Txk=Fxk−1+ωx̂ k=Fx̂ k−1e′=xk−x̂ k

真实值要考虑系统的噪音.于是 P 根据定义,真实值和估计值的误差的协方差,可以写成:

Pk=E[e′e′T]=E[(F(xk−1−x̂ k−1)+ω)(F(xk−1−x̂ k−1)+ω)T

omega 服从正态分布,根据协方差矩阵的性质wiki.P_k可以写成:

Pk=E[(Fek−1)(Fek−1)T]+E[ωkωTk]=FPk−1FT+Q

P 的初始值无所谓,会随迭代收敛.当前例子中 Q 应为对角阵,从本例和P的定义上理解,角度的误差和[角速度偏差的误差]当然无联系.所以副对角线上的值为0. Q 的主对角线是两个变量的方差.这说明若预测越不可靠(模型噪声大),P 的值越大(P 不一定增大,这里越大指的是相对可靠模型的迭代).
Q : 为什么整个过程没有算 P?
A: P 需要真实值,未知,所以无法从定义计算.

Step3-5

第三个方程

Kk=Pk|k−1HTS−1k

这部分核心功能是根据P 更新 K.K 是根据这个式子定义的:

ỹ k=zk−Hx̂ k|k−1x̂ =x̂ −+Kk(Zk−Hx̂ −)

上面也是第四个方程,应用 K 后验估计 x.x̂ −表示前两步的先验估计,x̂ .真正编程时完全可以用同一个变量.
H 我个人理解是观测和预测的坐标系尺度不一样,用来同步,本例中为1.K 表示信观测值的权重.K越大越逼近观测值.问题是 K 为多少时尽可能接近真实值.
参考资料<推导2,20页>做了这部分推导(不要看推导1).把 K 代入 P 矩阵,求导使方差最小即为 K 最优解.
始终记得 P 的意义.P 的对角线为 真实值和预测值的误差的方差.当这个值越小时说明预测值和真实值趋势越像.此时得到最优 K 解.
推导2用的符号和这里是一样的.多了个~号标记为误差,真实值和估计值直接的误差.

Q: 既然有 第五个方程 更新协方差矩阵为什么还要预测?
A: 第一步对状态向量做了预测.于是这两个变量相关的矩阵 P 也发生了变化.先对 P 做修正.再做测量优化.因为 P 无法直接计算得到,于是改变状态向量总是伴随着改变 P 的,这样才能保证 P 的迭代是正确的.

Step 6

KF第四个方程.

代码是这样的:

    // Calculate angle and bias - Update estimate with measurement zk (newAngle)    /* Step 3 角度残差 , 测量值-先验估计*/    float y = newAngle - angle; // Angle difference     /* Step 6 得到后验估计*/    angle += K[0] * y;    bias += K[1] * y;

angle 很好理解.bias += K * 角度残差 是怎么回事?
他原文里评论也很多也没搞懂,但是 lauszus 没有回应.我猜测bias 的残差部分(观测值-估计值)是按符号随意取的.因为偏移值无法直接测量.不断迭代后收敛,bias 估计值接近真实值了.

注意差分方程使用漂移值的方式:
θk=θk−1+Δt(θ˙−θ˙b)
bias 的单位是°/s. 漂移值应该是服从正态分布的.如果单独写一个程序不断读取陀螺仪角速度会发现静止时也有值.这个值就是漂移值.
这里系统使用差分方程,零输入时,漂移值应稳定,残差应不断收敛至0.此时 bias 的残差等于角度的残差(newAngle - angle).在未收敛时,bias 的残差符号和角度残差符号是一样的(不知道数学符号怎么表达,瞎写一下):

k=θk−θk−1=Δt(θ˙−θb˙)θ˙bk=θ˙bk−1+biasdiff

k 和θ˙−θb˙收敛至0.
若 k > 0,k递减, 则biasdiff > 0;
若 k < 0,k 递增,则biasdiff < 0;

做个实验:
改动Kalman.cpp getAngle 函数的这句表达式:

float y = newAngle - angle; // Angle difference/* Step 6 */angle += K[0] * y;bias += K[1] * y ;

变成:

float y = newAngle - angle; // Angle difference/* Step 6 */angle += K[0] * y;bias += K[1] * y *0.1;

未改动前采样一次,改动后采样一次.每次开机后转几圈传感器,让 K 值收敛,然后平放.
打印 K 值,bias值,和 newAngle 结果如下:

 k0 k1 bias  newRatedebuginfo :0.023    -0.025  0.110   0.036debuginfo :0.023    -0.025  0.115   0.043debuginfo :0.023    -0.025  0.116   0.066debuginfo :0.023    -0.025  0.120   0.158debuginfo :0.023    -0.025  0.124   -0.125  modify bias"s innovation update:debuginfo :0.023    -0.025    0.134    0.140    debuginfo :0.023    -0.025    0.135    0.201    debuginfo :0.023    -0.025    0.135    0.270    debuginfo :0.023    -0.025    0.134    0.193    debuginfo :0.023    -0.025    0.134    0.170    

发现改动后 bias 的残差(观测-估计)大小对结果影响并不大.而且 K 值偏小. 也就是静止时系统认为测量值不可靠.
这么做在模型静止时可行,但是加速运动时 bias 会突然增大,因为newAngle - Angle(使用k-1时刻的模型的角度估计) >0.此时 bias 的更新严重偏移,有时彪到7+ . 但是静止后,在 kF 作用下又降低到正常值.
我看另一个MPU6050的例子是直接静止时取10次平均值,作为固定 bias.
问题在于 bias真实值应该是固定还是和速度成正比?
静止时容易测量 lauszus 的模型是准的.但是运动时怎么验证 lauszus 是正确的?
//TODO 查看 K 值的波动.

Step 7

第五个方程用来更新 P. 前面说了由于 P 不能直接计算获得.每一次更新状态向量都要更新 P.P 满足:

将 Kk=Pk|k−1HTS−1k代入得:

Pk|k=(I−KkH)Pk|k−1

参数

总结一下上面几个链接对各个参数的理解:

• 初始估计值选取对最后收敛的估计值几乎没有影响。如果越相信测量的话，则初始估计协方差需要选取越大，如果感觉状态转移方程不够准确，则需要增大运动噪声.

@火枪手(小木虫):

1，关于kf的两个矩阵Q和R一般情况下不是对角阵，即存在互协方差分量，但是协方差矩阵一定是对称的不用解释吧。
2，R值参数选取。比如说状态向量是一个点的位置x,y，你的观测数据也是x,y，那么观测矩阵就是单位矩阵。对x作多次观测，取均值算方差的到rxx，对y同样处理得到ryy，那么R矩阵就是以rxx和ryy为对角的对角阵。
3，R矩阵说明。因为卡尔曼滤波假设是误差零均值，在第2点的例子中观测与状态同属于同一个坐标系，理想情况下R是对角阵，但是如果观测和状态不是同一坐标系的话，简单说就是观测量与状态量不相同，那么R一般不是对角阵，存在坐标变化的误差问题。
4，Q阵选取。Q是与系统模型F相关的，需要看你使用的模型是什么，依据该模型与真实状态之间的误差大概是多少来选择Q值，而且一般来说再建模的时候需要看到随机噪声，作离散化处理Q阵是被计算出来的，而不是随意拼凑的。同时，Q阵也属于虚拟噪声，类似于数字滤波器中的带宽的作用，也可以理解为所建模型和观测数据两者之间的信任程度，Q越大，越信任观测值，滤波输出值偏向等于观测值。