前言 【开发备忘|MPU6050卡尔曼滤波解算姿态角】自己在课上吹的牛，课程作业再麻烦也得干。模了好几天鱼，终于在DDL前一天弄完了惯导模块的简单demo，卡尔曼滤波算是我弄的最久的了（大概2-3天），虽然没有彻底弄懂原理（概率论没学，隐马尔可夫链啥的更是不会），好歹就着教程推导给实现了。
实现的效果不咋的，还是存在明显的跳动，估计是MPU6050陀螺仪的事，方差大、数值基本是不准的，爬了。
理论推导
假定条件 使用陀螺仪和加速度计实现卡尔曼滤波有几个基本假设，我码字的时候忘了几个，先列出来，以后想起来再填。
- 线性系统。对于稳态卡尔曼滤波，应该是线性时不变系统？
- 静态条件，也就是只有重力加速度G的情况下，才能使用加速度推导出欧拉角。
欧拉角 我们这里说的欧拉角是Z-Y-X欧拉角，也就是先绕Z轴旋转α角，再绕Y轴旋转β角，最后绕X轴旋转γ角。示意图大概是这么个样子。一般Z轴取铅垂轴，X轴为物体主对称轴，Y轴为辅对称轴或用右手定则确定
文章图片
那么我们就将绕Z轴旋转的角度Yaw称为偏航，绕X轴旋转的角度称为Roll滚转，绕Y轴旋转的角度Pitch称为俯仰角。
绕任意轴旋转的矩阵（Goldman公式） 欧拉角实际上是一种转角排列设定法，转角的排列顺序比较多，但是很多旋转实际上是一样的。我们可以找到一个等效的旋转轴和对应这个轴的转角，那么我们假定 K ^ 是 一 个 表 示 旋 转 轴 的 单 位 向 量 ， R K ( θ ) 为 等 效 旋 转 矩 阵 \hat{K}是一个表示旋转轴的单位向量，R_{K}(θ)为等效旋转矩阵 K^是一个表示旋转轴的单位向量，RK?(θ)为等效旋转矩阵
那么我们可以将被旋转向量V分解为沿轴分量V_c和V_1，利用向量叉积取垂直V_c和V_1的V_2。设旋转后的向量为V’,其在平面分量为V’_1，有： V ? = V c ? + V 1 ? \vec{V} = \vec{V_{c}}+\vec{V_{1}} V =Vc? ?+V1? ?
V ′ ? = V c ? + V 1 ′ ? \vec{V^{'}} =\vec{V_{c}}+\vec{V_{1}^{'}} V′ =Vc? ?+V1′? ?
示意图（意思意思就行了（狗头））
文章图片
下面就是用V_1和V_2表示旋转后的向量投影，假设转角为θ，θ实际上就是V’_1和V_1的夹角（图上忘画了），那么旋转后的向量就是：
V ’ ? = V c ? + V 1 ? c o s θ + V 2 ? s i n θ \vec{V^{’}}=\vec{V_{c}}+\vec{V_{1}}cosθ+\vec{V_{2}}sinθ V’ =Vc? ?+V1? ?cosθ+V2? ?sinθ
带入轴K的单位向量表示和各个坐标轴的坐标即可得到Goldman公式。例如：
K ^ = ( K x , K y , K z ) , V c ? = ( V c ? ? K ^ ) K ^ \hat{K} = (K_{x},K_{y},K_{z}),\vec{V_{c}}= (\vec{V_{c}}\cdot\hat{K})\hat{K} K^=(Kx?,Ky?,Kz?),Vc? ?=(Vc? ??K^)K^
如果令V=（1，0，0），即X轴，那么代入旋转后向量公式就可以求得矩阵的第一列。
最后的旋转矩阵应该是：
[ K x 2 ( 1 ? c ) + c K x K y ( 1 ? c ) ? K z s K x K z ( 1 ? c ) ? K y s K x K y ( 1 ? c ) + K z s K y 2 ( 1 ? c ) + c K y K z ( 1 ? c ) ? K x s K x K z ( 1 ? c ) ? K y s K z K y ( 1 ? c ) + K x s K z 2 ( 1 ? c ) + c ] \left[ \begin{matrix} K_{x}^{2}(1-c)+c & K_{x}K_{y}(1-c)-K_{z}s & K_{x}K_{z}(1-c)-K_{y}s \\ K_{x}K_{y}(1-c)+K_{z}s & K_{y}^{2}(1-c)+c & K_{y}K_{z}(1-c)-K_{x}s \\ K_{x}K_{z}(1-c)-K_{y}s & K_{z}K_{y}(1-c)+K_{x}s & K_{z}^{2}(1-c)+c \end{matrix} \right] ???Kx2?(1?c)+cKx?Ky?(1?c)+Kz?sKx?Kz?(1?c)?Ky?s?Kx?Ky?(1?c)?Kz?sKy2?(1?c)+cKz?Ky?(1?c)+Kx?s?Kx?Kz?(1?c)?Ky?sKy?Kz?(1?c)?Kx?sKz2?(1?c)+c????
其中c为cosθ，s为sinθ。
四元数/欧拉参数 基于上面的绕任意轴旋转的公式，我们可以使用4个数值表示旋转，这四个数值称为欧拉参数，可以视为一个单位四元数：
ε 1 = k x s i n θ 2 \varepsilon_{1} = k_{x}sin\frac{\theta}{2} ε1?=kx?sin2θ?
ε 2 = k y s i n θ 2 \varepsilon_{2} = k_{y}sin\frac{\theta}{2} ε2?=ky?sin2θ?
ε 3 = k z s i n θ 2 \varepsilon_{3} = k_{z}sin\frac{\theta}{2} ε3?=kz?sin2θ?
ε 4 = c o s θ 2 \varepsilon_{4} = cos\frac{\theta}{2} ε4?=cos2θ?
这四个参数满足：
ε 1 2 + ε 2 2 + ε 3 2 + ε 4 2 = 1 \varepsilon_{1}^{2}+\varepsilon_{2}^{2}+\varepsilon_{3}^{2}+\varepsilon_{4}^{2} =1 ε12?+ε22?+ε32?+ε42?=1
推导 说是推导，实际上就是说明几个符号的意义和算法的逻辑，真要看推导还得看参考文献1和3。
在这里，我们使用大写字母（比如X）表示矩阵，使用小写+斜体表示一个列向量,例如： x ^ k ? 1 ∣ k ? 1 和 x ^ k ∣ k ? 1 \boldsymbol{\hat{x}_{k-1|k-1}}和\boldsymbol{\hat{x}_{k|k-1}} x^k?1∣k?1?和x^k∣k?1?
注意上面两个例子的下标和上标，\hat表示该值/向量为预测得到的，而k-1|k-1表示k-1时刻该值/向量为经过k-1时刻卡尔曼滤波计算后得到，k|k-1则表明这是利用k-1时刻对k时刻进行的预测（没有经过卡尔曼滤波）。
首先建立系统的状态空间方程（应该是这么说？）
x k = F x k ? 1 + B u k + w k — — ( 1 ) \boldsymbol{{x}_{k}} = F\boldsymbol{{x}_{k-1}+B\boldsymbol{{u}_{k}}+\boldsymbol{{w}_{k}}} ——(1) xk?=Fxk?1?+Buk?+wk?——(1)
z k = H x k + v k — — ( 2 ) \boldsymbol{{z}_{k}} = H\boldsymbol{{x}_{k}}+\boldsymbol{{v}_{k}} ——(2) zk?=Hxk?+vk?——(2)
式中： x k 为 k 时 刻 系 统 的 状 态 向 量 （ 同 时 也 是 你 需 要 的 滤 波 结 果 向 量 ） \boldsymbol{{x}_{k}}为k时刻系统的状态向量（同时也是你需要的滤波结果向量） xk?为k时刻系统的状态向量（同时也是你需要的滤波结果向量）
u k 为 k 时 刻 系 统 的 输 入 向 量 （ 也 就 是 输 入 的 控 制 量 ） , 而 z k 为 观 测 \boldsymbol{{u}_{k}}为k时刻系统的输入向量（也就是输入的控制量）,而\boldsymbol{{z}_{k}}为观测 uk?为k时刻系统的输入向量（也就是输入的控制量）,而zk?为观测
向 量 ( 一 般 与 x 不 同 ) ; w k 为 过 程 噪 声 , w k ～ N ( 0 , Q k ) , Q k 为 过 程 噪 向量(一般与\boldsymbol{x}不同); \boldsymbol{{w}_{k}}为过程噪声,\boldsymbol{{w}_{k}}\sim N(0,\boldsymbol{Q_{k}}),\boldsymbol{Q_{k}}为过程噪 向量(一般与x不同); wk?为过程噪声,wk?～N(0,Qk?),Qk?为过程噪
声 协 方 差 矩 阵 ； v k 为 观 测 噪 声 , v k ～ N ( 0 , R k ) , R k 为 观 测 噪 声 协 声协方差矩阵；\boldsymbol{{v}_{k}}为观测噪声,\boldsymbol{{v}_{k}}\sim N(0,\boldsymbol{R_{k}}),\boldsymbol{R_{k}}为观测噪声协 声协方差矩阵；vk?为观测噪声,vk?～N(0,Rk?),Rk?为观测噪声协
方 差 矩 阵 ； F 表 示 k ? 1 时 刻 对 k 时 刻 的 影 响 ， B 表 示 输 入 对 状 态 的 方差矩阵；F表示k-1时刻对k时刻的影响，B表示输入对状态的 方差矩阵；F表示k?1时刻对k时刻的影响，B表示输入对状态的
影 响 ; H 将 状 态 向 量 映 射 到 观 测 向 量 空 间 ， 是 一 个 线 性 映 射 。 影响; H将状态向量映射到观测向量空间，是一个线性映射。 影响; H将状态向量映射到观测向量空间，是一个线性映射。
基于（1）（2）式，我就开始推导(列公式)了。首先是预测过程，如果不考虑噪声，我们的预测是：
x ^ k ∣ k ? 1 = F x ^ k ? 1 ∣ k ? 1 + B u k — — ( 3 ) \boldsymbol{\hat{{x}}_{k|k-1}}= F\boldsymbol{\hat{{x}}_{k-1|k-1}+B\boldsymbol{{u}_{k}}} ——(3) x^k∣k?1?=Fx^k?1∣k?1?+Buk?——(3)
然后是对于协方差矩阵P进行预测，协方差矩阵P的定义是：
P k = C o v ( x k ? x ^ k ∣ k ? 1 , x k ? x ^ k ∣ k ? 1 ) \boldsymbol{P_{k}}= Cov(\boldsymbol{\boldsymbol{{x}_{k}}-\hat{{x}}_{k|k-1},\boldsymbol{\boldsymbol{{x}_{k}}-\hat{{x}}_{k|k-1}})} Pk?=Cov(xk??x^k∣k?1?,xk??x^k∣k?1?)
对(1)和(3)做差，然后由于Q和预测无关，我们就可以得到对P的预测值:
P k ∣ k ? 1 = F P k ? 1 ∣ k ? 1 F T + Q — — ( 4 ) \boldsymbol{{P}_{k|k-1}}= F\boldsymbol{{P}_{k-1|k-1}}F^{T}+Q ——(4) Pk∣k?1?=FPk?1∣k?1?FT+Q——(4)
对于MPU6050，我们设置观测变量为：
x k = [ θ θ ˙ b i a s ] ， θ ˙ b i a s 为 角 速 度 偏 差 值 \boldsymbol{{x}_{k}} = \left[\begin{matrix} \theta \\ \dot{\theta}_{bias} \end{matrix}\right]， \dot{\theta}_{bias}为角速度偏差值 xk?=[θθ˙bias??]，θ˙bias?为角速度偏差值
显然，这两个是不相干的，因此协方差矩阵Q为一个对角阵，输入变量U为：
u k = θ ˙ ， θ ˙ 为 角 速 度 \boldsymbol{{u}_{k}} = \dot{\theta} ， \dot{\theta}为角速度 uk?=θ˙，θ˙为角速度
那么矩阵F、B、Q为,dt为采样时间，Q_{θ}为角度的协方差：
F = [ 1 d t 0 1 ] ; B = [ d t 0 ] ; Q = [ Q θ 0 0 Q θ ˙ b ] ? d t \boldsymbol{F} = \left[\begin{matrix} 1 &dt \\ 0 & 1 \end{matrix}\right]; \boldsymbol{B} = \left[\begin{matrix} dt \\ 0 \end{matrix}\right]; \boldsymbol{Q} = \left[\begin{matrix} Q_{\theta} &0 \\ 0 & Q_{\dot\theta_{b}} \end{matrix}\right]\cdot dt F=[10?dt1?]; B=[dt0?]; Q=[Qθ?0?0Qθ˙b???]?dt
关于协方差矩阵P，其初始化值其实不需要非常关心。因为如果是线性时不变系统，在经过一段时间后P会迭代到一个稳态的值，初值只是会改变这个迭代的速度。如果初始值是精确的，那么完全可以初始为一个0阵；对于我们这样设置的状态变量，因为其相互独立，所以P也是个对角阵，P可以初始化为,为一个比较大的值：
P = [ L 0 0 L ] \boldsymbol{P}= \left[\begin{matrix} L &0 \\ 0 & L \end{matrix}\right] P=[L0?0L?]
下面是观测：
根据（2）式，我们先来讨论一下向量和矩阵。
为了便于在C语言中实现，我们这里用的是二维的单独解算，如果我们设置观测向量为单纯某个角度（比如滚转角Roll），那么就不需要求矩阵的逆（具体原因后面说），大大简化运算。
因此，观测向量为一个角度标量，这个角度通过加速度算出；同样，观测噪声矩阵R也为一个标量。
z k = f ( A c c x k , A c c y k , A c c z k ) ; H = [ 1 0 ] \boldsymbol{z_{k}} = f(\boldsymbol{Accx_{k}},\boldsymbol{Accy_{k}},\boldsymbol{Accz_{k}}); \boldsymbol{H}=\left[\begin{matrix} 1 &0 \end{matrix}\right] zk?=f(Accxk?,Accyk?,Acczk?); H=[1?0?]
关于R的值，R越大，证明我们对观测结果越不信任，会使得响应变慢；如果过小，那么噪声影响就会比较大。
观测我们先需要通过加速度获取观测值，这里用参考博文2 的atan2那个方法进行计算，具体可以看那篇博文。
然后计算预计和观测的误差,显然这个误差是个标量(scalar)：
y k = z k ? H x ^ k ∣ k ? 1 \boldsymbol{y_{k}} =\boldsymbol{z_{k}} -H\boldsymbol{\hat{x}_{k|k-1}} yk?=zk??Hx^k∣k?1?
然后为了方便计算，我们将一个中间变量定义为矩阵S，在参考博文1中作者称其为innovation covariance，我也不知道是啥。
S = H P k ∣ k ? 1 H T + R S = HP_{k|k-1}H^{T}+R S=HPk∣k?1?HT+R
在我们这个情况下，这个矩阵S是一个标量，因此后面对其求逆时只需要做除法（填坑）。
下面进行算法的下一步：计算卡尔曼增益：
K k = P k ∣ k ? 1 H T S ? 1 — — ( 5 ) K_{k} = P_{k|k-1}H_{T}S^{-1}——(5) Kk?=Pk∣k?1?HT?S?1——(5)
对 于 我 们 具 体 实 现 ， 可 以 写 为 ： K k = P k ∣ k ? 1 H T [ S ] 对于我们具体实现，可以写为：K_{k} = \frac{P_{k|k-1}H^{T}} {[S]} 对于我们具体实现，可以写为：Kk?=[S]Pk∣k?1?HT?
有了这一步的卡尔曼增益，我们就可以对预测和观测进行Fusion了：
x ^ k ∣ k = x ^ k ∣ k ? 1 + K k y ^ k — — ( 6 ) \boldsymbol{\hat{x}_{k|k}} =\boldsymbol{\hat{x}_{k|k-1}} +K_{k}\boldsymbol{\hat{y}_{k}} ——(6) x^k∣k?=x^k∣k?1?+Kk?y^?k?——(6)
最后一步就是实现协方差矩阵P的更新：
P k ∣ k = ( I ? K k H ) P k ∣ k ? 1 — — ( 7 ) \boldsymbol{P_{k|k}} =(I-K_{k}H)\boldsymbol{P_{k|k-1}} ——(7) Pk∣k?=(I?Kk?H)Pk∣k?1?——(7)
卡尔曼滤波调参：
调参这个问题，我不是很明白，只能大概说一下，可能没啥用，反正这篇博文是为以后开发做备忘。
参数有：协方差矩阵P的初始值，状态向量x的初始值，过程噪声矩阵Q，测量噪声矩阵R
对于状态向量x的初始值，其实也不是一个参数，因为初始情况一般已知。如果说具体实现，那就保持静止采几百个样，求均值基本就行。
对于P的初值，其实没太有影响，P的初始值只是影响卡尔曼滤波的收敛速度，如果对初始观测十分确定，取0阵就行。
比较难确定的就是过程噪声矩阵Q和测量噪声矩阵R，这里的参数我是抄第一篇博文的，具体怎样通过实验测定我也不是很清楚。
卡尔曼滤波实质：
对于一维的情况来说，卡尔曼滤波实际上是两个正太分布的乘积\叠加，我们通过kalman得到的估计值实际上就是这个叠加的新期望值，具体的可以看参考文章3.
C实现 这个网上很多，博主赶着打游戏，有空再填（狗头）。
Python实现
import numpy as np
import matplotlib.pyplot as plt
import math
#compute the sigma of accel and gryo's measurement
dataset =np.loadtxt(r'C:\Users\Lin\Desktop\ProjectFisher\navigation\ckalman.csv',delimiter=',')
sigma = np.std(dataset,axis = 0)
print(sigma)
print(dataset.mean(axis = 0))
gryo_x = dataset[:,0]
accel_z = dataset[:,5]
accel_y = dataset[:,4]
accel_x = dataset[:,3]
RollC = dataset[:,6]
K1C = dataset[:,7]
K2C = dataset[:,8]
n = accel_x.shape[0]
#kalman compute
'''
对于MPU6050进行卡尔曼滤波的前提条件：
1. 线性时不变系统
2. 铅坠方向加速度为一定值，如果不是，那么应该用磁强计进行角度观测
3. 以后再补充
'''
TimeList = [i/2 for i in range(0,n,1)]#时间序列
GAngle = 0
GAngleList = []#预测得到的角度
AAngleList = []#观测值得到的角度
KAngleList = []#Kalman滤波后的角度
KalmanList = []#Kalman 增益值1
KalmanList2 = []#Kalman 增益值2
BiasList = []
angle = 0#初始角度为0
bias = 0 #初始的角速度偏差为0#参量：dt采样时间，P初始协方差矩阵（与收敛有关），Q过程噪声矩阵（越大预测可信度越低），测量噪声矩阵R（R越大观测可信度越低）
dt = 0.5
X = np.array([[angle],[bias]]) #预测变量
Gryo = 0 #输入控制变量
Z = 0 #观测量，为加速度计算出的角度.实际上，加速度计算出的角度，只能说铅锤轴的加速度g不变推导出的
Y =0#观测量与预计值的误差，Y = Z-HX
P = np.array([[1,0],[0,1]]) #初始化协方差矩阵P,P是一个对角阵，[L,0;
0,L]L是一个很大的数；如果已知初始状态，那么认为置信度100%，即为0阵
Q = np.array([[0.001,0],[0,0.17]])#过程噪声矩阵，角度的协方差来自TKJ，角速度为测量得到的方差，实际上真正的过程噪声矩阵还应该xdt
R = 0.01 #测量噪声矩阵，由于是二维且只有角度，因此转换为标量，使用加速度的方差F = np.array([[1,-dt],[0,1]])
B = np.array([[dt],[0]])#输入作用于预测的矩阵B
H= np.array([[1,0],])#状态转移矩阵，将预测空间的变量映射到观测空间，这里的观测是角度（使用加速度分量计算而得出）
K = np.array([[0],[0]])#卡尔曼增益
I =np.array([[1,0],[0,1]])#单位矩阵for i in range(0,n,1):
Gryo = gryo_x[i]#计算预测得到的结果
GAngle += Gryo*dt
GAngleList.append(GAngle)X =np.dot(F,X)+np.dot(B,Gryo) # 根据k-1时刻计算k时刻预测值
P = np.dot(np.dot(F,P),F.T)+Q*dt #根据k-1时刻估计的协方差矩阵
Roll = math.degrees(math.atan2(accel_y[i],accel_z[i]))
#Roll角还可以通过atan(accY/sqrt(accX^2+accZ^2))计算,网上的欧拉角计算有两种方式，
#参考https://blog.csdn.net/qcopter/article/details/51848544
Z = Roll
AAngleList.append(Z)#计算单纯观测得到的结果
Y =Z - np.dot(H,X)
S = np.dot(np.dot(H,P),H.T)+R #中间变量，S理论上应该是矩阵，在最后计算卡尔曼增益时需要取inv（S），但是这里S是scalar，因此求逆只需要做除法
K = np.dot(P,H.T)/S#计算卡尔曼增益
X = X+K*Y #卡尔曼滤波后的预测值
P = np.dot(I-np.dot(K,H),P)
KAngleList.append(X[0][0])
KalmanList.append(K[0][0])
KalmanList2.append(K[1][0])
BiasList.append(X[1][0])
KAngleList = np.array(KAngleList)
KalmanList = np.array(KalmanList)
GAngleList = np.array(GAngleList)
AAngleList = np.array(AAngleList)
BiasList = np.array(BiasList)plt.figure("kalmanfilter")
plt.subplot(231)
plt.plot(TimeList,KAngleList,'r--')
plt.plot(TimeList,RollC,'c')
plt.subplot(232)
plt.plot(TimeList,GAngleList,'g')
plt.subplot(233)
plt.plot(TimeList,AAngleList,'b')
plt.subplot(234)
plt.plot(TimeList,gryo_x,'y')
plt.subplot(235)
plt.plot(TimeList,BiasList,'y--')
plt.subplot(236)plt.plot(TimeList,KalmanList, 'r--')
plt.plot(TimeList,K1C, 'c')
plt.plot(TimeList,KalmanList2, 'g--')
plt.plot(TimeList,K2C, 'k')
plt.show()与C实现的结果进行对比：
第一张虚线是Python实现的卡尔曼滤波结果，天蓝色是C语言实现版本结果；第二张是单纯使用陀螺仪积分结果，第三张是单独使用加速度解算结果；第四张是角速度；第六章实线是C语言计算出的卡尔曼增益，虚线是Python计算出的。两者曲线基本重合，所以应该是没问题的，但是为什么第一张滤波结果不同，估计是计算精度的问题。
文章图片
Reference
- 卡尔曼滤波算法的推导以及C实现（这篇博文强烈推荐，说的特别详细）：a-practical-approach-to-kalman-filter-and-how-to-implement-it
- 关于姿态角的加速度求解：静态条件下三轴加速度求角度的算法
- 卡尔曼滤波算法的参数理解与推导？：卡尔曼滤波算法的参数理解
- Goldman公式的推导：Goldman公式的推导
- 机器人学导论[J.Craig]
