消炎痛栓MPU6050卡尔曼滤波解算姿态⾓
前⾔
⾃⼰在课上吹的⽜，课程作业再⿇烦也得⼲。模了好⼏天鱼，终于在DDL前⼀天弄完了惯导模块的简单demo，卡尔曼滤波算是我弄的最久的了（⼤概2-3天），虽然没有彻底弄懂原理（概率论没学，隐马尔可夫链啥的更是不会），好⽍就着教程推导给实现了。 2011山东理综实现的效果不咋的，还是存在明显的跳动，估计是MPU6050陀螺仪的事，⽅差⼤、数值基本是不准的，爬了。
理论推导
假定条件
使⽤陀螺仪和加速度计实现卡尔曼滤波有⼏个基本假设，我码字的时候忘了⼏个，先列出来，以后想起来再填。
1. 线性系统。对于稳态卡尔曼滤波，应该是线性时不变系统？
2. 静态条件，也就是只有重⼒加速度G的情况下，才能使⽤加速度推导出欧拉⾓。
欧拉⾓和四元数
这个部分其实只⽤了解⼀下欧拉⾓即可，但是作为⼀个开发备忘，绕任意轴旋转的矩阵、四元数还是要有的，万⼀以后⽤到了呢（狗头）。 欧拉⾓
我们这⾥说的欧拉⾓是Z-Y-X欧拉⾓，也就是先绕Z轴旋转α⾓，再绕Y轴旋转β⾓，最后绕X轴旋转γ⾓。⽰意图⼤概是这么个样⼦。⼀般Z轴取铅垂轴，X轴为物体主对称轴，Y轴为辅对称轴或⽤右⼿定则确定
那么我们就将绕Z轴旋转的⾓度Yaw称为偏航，绕X轴旋转的⾓度称为Roll滚转，绕Y轴旋转的⾓度Pitch称为俯仰⾓。
绕任意轴旋转的矩阵（Goldman公式）
欧拉⾓实际上是⼀种转⾓排列设定法，转⾓的排列顺序⽐较多，但是很多旋转实际上是⼀样的。我们可以到⼀个等效的旋转轴和对应这个轴的转⾓，那么我们假定
那么我们可以将被旋转向量V分解为沿轴分量V_c和V_1，利⽤向量叉积取垂直V_c和V_1的V_2。设旋转后的向量为V’，其在平⾯分量为V’_1，有： ⽰意图（意思意思就⾏了（狗头））
下⾯就是⽤V_1和V_2表⽰旋转后的向量投影，假设转⾓为θ，θ实际上就是V’_1和V_1的夹⾓（图上忘画了），那么旋转后的向量就是：
带⼊轴K的单位向量表⽰和各个坐标轴的坐标即可得到Goldman公式。例如：
如果令V=（1，0，0），即X轴，那么代⼊旋转后向量公式就可以求得矩阵的第⼀列。
最后的旋转矩阵应该是：
其中c为cosθ，s为sinθ。
四元数/欧拉参数
是⼀个表⽰旋转轴的单位向量，R (θ)为等效旋转矩阵K
^K =V +V c V 1
=V ′+V c V 1′=V ’+V c cosθ+V 1sinθ
V 2=K
^(K ,K ,K ),=x y z V c (⋅V c )K ^K ^⎣⎡K (1−c )+c x 2
K K (1−c )+K s x y z K K (1−c )−K s x z y K K (1−c )−K s x y z K (1−c )+c y 2K K (1−c )+K s z y x K K (1−c )−K s x z y K K (1−c )−K s y z x K (1−c )+c z 2⎦⎤
基于上⾯的绕任意轴旋转的公式，我们可以使⽤4个数值表⽰旋转，这四个数值称为欧拉参数，可以视为⼀个单位四元数：
这四个参数满⾜：
推导
说是推导，实际上就是说明⼏个符号的意义和算法的逻辑，真要看推导还得看参考⽂献1和3。
在这⾥，我们使⽤⼤写字母（⽐如X）表⽰矩阵，使⽤⼩写+斜体表⽰⼀个列向量，例如：
注意上⾯两个例⼦的下标和上标，\hat表⽰该值/向量为预测得到的，⽽k-1|k-1表⽰k-1时刻该值/向量为经过k-1时刻卡尔曼滤波计算后得到，k|k-1则表明这是利⽤k-1时刻对k时刻进⾏的预测（没有经过卡尔曼滤波）。
ε=1k sin x 2
θ
ε=2k sin y 2
θ
ε=3k sin z 2
θ
ε=4cos 2
θ
ε+12ε+22ε+32ε=42
1和x ^k −1∣k −1x ^k ∣k −1
⾸先建⽴系统的状态空间⽅程（应该是这么说？）
式中：基于（1）（2）式，我就开始推导(列公式)了。⾸先是预测过程，如果不考虑噪声，我们的预测是：
然后是对于协⽅差矩阵P进⾏预测，协⽅差矩阵P的定义是：
对(1)和(3)做差，然后由于Q和预测⽆关，我们就可以得到对P的预测值:
显然，这两个是不相⼲的，因此协⽅差矩阵Q为⼀个对⾓阵，输⼊变量U为：
x =k F x +Bu +w ——(1)
k −1k k z =k H x +k v ——(2)
k x 为k 时刻系统的状态向量（同时也是你需要的滤波结果向量）
k u 为k 时刻系统的输⼊向量（也就是输⼊的控制量）,⽽z 为观测
k k 向量(⼀般与x 不同);w 为过程噪声,w ∼k k N (0,Q ),Q 为过程噪
k k 声协⽅差矩阵；v 为观测噪声,v ∼k k N (0,R ),R 为观测噪声协
k k ⽅差矩阵；F 表⽰k −1时刻对k 时刻的影响，B 表⽰输⼊对状态的
稀土在线
影响;H 将状态向量映射到观测向量空间，是⼀个线性映射。
=x ^k ∣k −1F +Bu ——(3)
x ^k −1∣k −1k P =k Cov (x −,x −)
k x ^k ∣k −1k x ^k ∣k −1P =k ∣k −1F P F +k −1∣k −1T Q ——(4)
x =k ，为⾓速度偏差值[θ
θ˙bias ]θ˙bias u =k ，为⾓速度θ
˙θ˙
那么矩阵F、B、Q为,dt为采样时间，Q_{θ}为⾓度的协⽅差：
关于协⽅差矩阵P，其初始化值其实不需要⾮常关⼼。因为如果是线性时不变系统，在经过⼀段时间后P会迭代到⼀个稳态的值，初值只是会改变这个迭代的速度。如果初始值是精确的，那么完全可以初始为⼀个0阵；对于我们这样设置的状态变量，因为其相互独⽴，所以P也是个对⾓阵，P可以初始化为，为⼀个⽐较⼤的值：
动机归因理论
下⾯是观测：
根据（2）式，我们先来讨论⼀下向量和矩阵。
为了便于在C语⾔中实现，我们这⾥⽤的是⼆维的单独解算，如果我们设置观测向量为单纯某个⾓度（⽐如滚转⾓Roll），那么就不需要求矩阵的逆（具体原因后⾯说），⼤⼤简化运算。
因此，观测向量为⼀个⾓度标量，这个⾓度通过加速度算出；同样，观测噪声矩阵R也为⼀个标量。
关于R的值，R越⼤，证明我们对观测结果越不信任，会使得响应变慢；如果过⼩，那么噪声影响就会⽐较⼤。
观测我们先需要通过加速度获取观测值，这⾥⽤参考博⽂2 的atan2那个⽅法进⾏计算，具体可以看那篇博⽂。
然后计算预计和观测的误差，显然这个误差是个标量(scalar)：
然后为了⽅便计算，我们将⼀个中间变量定义为矩阵S，在参考博⽂1中作者称其为innovation covariance ，我也不知道是啥。
在我们这个情况下，这个矩阵S是⼀个标量，因此后⾯对其求逆时只需要做除法（填坑）。
下⾯进⾏算法的下⼀步：计算卡尔曼增益：
有了这⼀步的卡尔曼增益，我们就可以对预测和观测进⾏Fusion了：麝香仁
最后⼀步就是实现协⽅差矩阵P的更新：
F =;B =[10dt 1];Q =[dt 0]⋅[Q θ00Q θ˙b ]dt
P =[L 00
L ]
z =k f (Accx ,Accy ,Accz );H =k k k [10]
y =k z −k H x ^k ∣k −1
S =HP H +k ∣k −1T R
K =k P H S ——(5)
北京人资k ∣k −1T −1对于我们具体实现，可以写为：K =
k [S ]P H k ∣k −1T
=x ^k ∣k +x ^k ∣k −1K ——(6)k y
^k P =k ∣k (I −K H )P ——(7)k k ∣k −1
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议