卡尔曼滤波器资料整理

卡尔曼滤波器实现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\boldsymbol{x}_k = \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_k
F	状态转移矩阵，和具体的线性系统相关
uk	K时刻外界对系统的作用,[θ˙k,0]T[\dot{\theta}_k , 0]^T
θ\theta	角度
θb˙\dot{\theta_b}	角速度偏差
θ˙\dot{\theta}	角速度
B	输入控制矩阵，外界的影响如何转化为对状态的影响
P	误差矩阵 , 预测值和真实值的误差的协方差方程.\n	因为当前状态向量有两个变量(角度和角速度),\n 所以此为2x2矩阵.记住主对角线代表两个变量的方差,副对角线代表误差相关程度
Q	预测噪声协方差矩阵
R	测量噪声协方差矩阵
H	观测矩阵
Kk	K时刻的kalman增益
zk	K时刻的观测值

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

Step1:

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

xk=Fxk−1+Buk+wk

\boldsymbol{x}_k = \boldsymbol{F}x_{k-1} + \boldsymbol{B}u_k + w_k

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

\begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k | k-1} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \theta \\ \dot{\theta}_b \end{bmatrix}_{k-1 | k-1} + \begin{bmatrix} \Delta t \\ 0 \end{bmatrix} \dot{\theta}_k
关键在于建模要准确.模型预测要尽可能贴近真实值.如果建模不准 , 可能整体滤波结果落后与真实值.见链接的实验.角速度偏差 θb˙\dot{\theta_b}后面讨论.

Step2

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

\boldsymbol{P}_{k | k-1} = \boldsymbol{F}\boldsymbol{P}_{k-1 | k-1}\boldsymbol{F}^T + \boldsymbol{Q}_k
然后第二方程,误差协方差矩阵P 的预测方程是推导出来的.但是 lauszus 只说这是定义.参考链接重写一下这个推导过程:
假设真实值为 x.用我们的模型表示,则零输入下为:

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

x_k = [\theta , 0]^T \\ x_k = F x_{k-1} + \omega \\ \hat{x}_k = F \hat{x}_{k-1} \\ e' = x_k - \hat{x}_k
真实值要考虑系统的噪音.于是 P 根据定义,真实值和估计值的误差的协方差,可以写成:

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

P_k = E[e' e'^T] = E[( F(x_{k-1}-\hat{x}_{k-1} )+ \omega ) ( F(x_{k-1}-\hat{x}_{k-1} )+ \omega )^T
omega 服从正态分布,根据协方差矩阵的性质 wiki.P_k可以写成:

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

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

Step3-5

第三个方程

Kk=Pk|k−1HTS−1k

\boldsymbol{K}_k = \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T \boldsymbol{S}_k^{-1}
这部分核心功能是根据P 更新 K.K 是根据这个式子定义的:

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

\boldsymbol{\tilde{y}}_k = \boldsymbol{z}_k - \boldsymbol{H}\hat{x}_{k | k-1} \\ \hat{x}=\hat{x}^- +K_k(Z_k-H\hat{x}^-)
上面也是第四个方程,应用 K 后验估计 x. x̂ −\hat{x}^-表示前两步的先验估计, x̂ \hat{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)\theta_k = \theta_{k-1} + \Delta t (\dot{\theta} - \dot{\theta}_b)
bias 的单位是°/s. 漂移值应该是服从正态分布的.如果单独写一个程序不断读取陀螺仪角速度会发现静止时也有值.这个值就是漂移值.
这里系统使用差分方程,零输入时,漂移值应稳定,残差应不断收敛至0.此时 bias 的残差等于角度的残差(newAngle - angle).在未收敛时,bias 的残差符号和角度残差符号是一样的(不知道数学符号怎么表达,瞎写一下):

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

k=\theta_k - \theta_{k-1} = \Delta t (\dot{\theta} - \dot{\theta_b})\\ \dot{\theta}_{bk} =\dot{\theta}_{bk-1} + biasdiff
k 和 θ˙−θb˙\dot{\theta} - \dot{\theta_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.036
debuginfo :0.023    -0.025  0.115   0.043
debuginfo :0.023    -0.025  0.116   0.066
debuginfo :0.023    -0.025  0.120   0.158
debuginfo :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\boldsymbol{K}_k = \boldsymbol{P}_{k | k-1} \boldsymbol{H}^T \boldsymbol{S}_k^{-1} 代入得:

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

\boldsymbol{P}_{k | k} = (\boldsymbol{I} - \boldsymbol{K}_k \boldsymbol{H}) \boldsymbol{P}_{k | 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越大，越信任观测值，滤波输出值偏向等于观测值。

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

六轴传感器基础知识学习：MPU6050特性，四元数，姿态解算，卡尔曼滤波
实际上,只要说到多少轴的传感器一般是就是指加速度传感器(即加速计).角速度传感器(即陀螺仪).磁感应传感器(即电子罗盘).这三类传感器测量的数据在空间坐标系中都可以被分解为X,Y,Z三个方向轴的力,因 ...
【STM32Cube】学习笔记（三）：六轴传感器
文章目录摘要一.简介 1.I2C原理 2.MPU6050介绍 3.MPU6050寄存器介绍 4.DMP使用二.硬件电路设计三.软件设计 1.CubeMX配置 2.CubeIDE代码 3.结果显 ...
Micro Python———MPU6050六轴传感器
目录 1.什么是MPU6050? MPU6050介绍: MPU6050寄存器介绍: 2.例程 1.平台 2.目的 3.讲解 1.查阅原理图 2.流程分析 3.代码讲解 3.结果 1.什么是MPU605 ...
C语言 | 基于MPU605(六轴传感器)的I2C实现LCD1602显示（代码类）
博主github:https://github.com/MichaelBeechan 博主CSDN:https://blog.csdn.net/u011344545 基于MPU605(六轴传感器)的I ...
keil4怎么移植其他人的程序_【调试笔记】韦东山：在100ask_imx6ull上移植使用六轴传感器ICM20608...
之前发了LCD调试笔记,大家很感兴趣,所以这次再来一篇:六轴传感器ICM20608驱动移植笔记,大家还需要什么移植笔记?可以留言.我们尽量满足. 1.1 移植思路先找到驱动:也许内核里已经有,也许需 ...
LSM6DS3（六轴传感器）STM32驱动及6D功能实现
刚使用了STM32测试了LSM6DS3该六轴传感器,顺便也测试了其6D方向检测功能,效果是能满足对6个面方向的识别需求. 该六轴传感器支持I2C/SPI通信.在读取该六轴传感器寄存器时,采用I2C通信 ...
【开源教程13】疯壳·开源编队无人机-SPI（六轴传感器数据获取）
COCOFLY教程 --疯壳·无人机·系列 SPI(六轴传感器数据获取) 图1 一.ICM20602 简介六轴传感器在当今智能穿戴和定位导航产品 ...
【飞控开发基础教程6】疯壳·开源编队无人机-SPI（六轴传感器数据获取）
COCOFLY教程 --疯壳·无人机·系列 SPI(六轴传感器数据获取) 图1 一.ICM20602 简介六轴传感器在当今智能穿戴和定位导航产品 ...
ICM20602六轴传感器-IIC通信模式
ICM20602六轴传感器 ICM20602 通过IIC协议与MCU通信 ICM20602 初始化配置 ICM20602 相关配置函数 ICM20602 内部寄存器注意事项 (一)ICM20602 ...
android六轴传感器,6轴传感器、IP67防水：AMAZFIT米动智芯2 上架有品
6轴传感器.IP67防水:AMAZFIT米动智芯2 上架有品49元 2018-07-09 10:23:57 29点赞 35收藏 39评论直达链接作为智能跑鞋的核心,智能运动芯片通常会随着运动鞋一起 ...

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