MPU6050卡尔曼滤波解算姿态角

前言

自己在课上吹的牛，课程作业再麻烦也得干。模了好几天鱼，终于在DDL前一天弄完了惯导模块的简单demo，卡尔曼滤波算是我弄的最久的了（大概2-3天），虽然没有彻底弄懂原理（概率论没学，隐马尔可夫链啥的更是不会），好歹就着教程推导给实现了。
实现的效果不咋的，还是存在明显的跳动，估计是MPU6050陀螺仪的事，方差大、数值基本是不准的，爬了。

理论推导

假定条件

使用陀螺仪和加速度计实现卡尔曼滤波有几个基本假设，我码字的时候忘了几个，先列出来，以后想起来再填。

1. 线性系统。对于稳态卡尔曼滤波，应该是线性时不变系统？
2. 静态条件，也就是只有重力加速度G的情况下，才能使用加速度推导出欧拉角。

欧拉角和四元数

这个部分其实只用了解一下欧拉角即可，但是作为一个开发备忘，绕任意轴旋转的矩阵、四元数还是要有的，万一以后用到了呢（狗头）。

欧拉角

我们这里说的欧拉角是Z-Y-X欧拉角，也就是先绕Z轴旋转α角，再绕Y轴旋转β角，最后绕X轴旋转γ角。示意图大概是这么个样子。一般Z轴取铅垂轴，X轴为物体主对称轴，Y轴为辅对称轴或用右手定则确定
那么我们就将绕Z轴旋转的角度Yaw称为偏航，绕X轴旋转的角度称为Roll滚转，绕Y轴旋转的角度Pitch称为俯仰角。

绕任意轴旋转的矩阵（Goldman公式）

欧拉角实际上是一种转角排列设定法，转角的排列顺序比较多，但是很多旋转实际上是一样的。我们可以找到一个等效的旋转轴和对应这个轴的转角，那么我们假定$$\hat{K}是一个表示旋转轴的单位向量，R_K(\theta )为等效旋转矩阵$$
那么我们可以将被旋转向量V分解为沿轴分量V_c和V_1，利用向量叉积取垂直V_c和V_1的V_2。设旋转后的向量为V’,其在平面分量为V’_1，有：$$V=V_c+V_1$$
$$V^{{}^{\prime }}=V_c+V_1^{{}^{\prime }}$$
示意图（意思意思就行了（狗头））
下面就是用V_1和V_2表示旋转后的向量投影，假设转角为θ，θ实际上就是V’_1和V_1的夹角（图上忘画了），那么旋转后的向量就是：
$$V^{’}=V_c+V_{1}cos\theta +V_{2}sin\theta$$
带入轴K的单位向量表示和各个坐标轴的坐标即可得到Goldman公式。例如：
$$\hat{K}=(K_x,K_y,K_z),V_c=(V_c\cdot \hat{K})\hat{K}$$
如果令V=（1，0，0），即X轴，那么代入旋转后向量公式就可以求得矩阵的第一列。
最后的旋转矩阵应该是：
$$\left[\begin{array}{ccc}{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{array}\right]$$
其中c为cosθ，s为sinθ。

四元数/欧拉参数

基于上面的绕任意轴旋转的公式，我们可以使用4个数值表示旋转，这四个数值称为欧拉参数，可以视为一个单位四元数：
$${\epsilon }_1=k_{x}sin\frac{\theta }{2}$$
$${\epsilon }_2=k_{y}sin\frac{\theta }{2}$$
$${\epsilon }_3=k_{z}sin\frac{\theta }{2}$$
$${\epsilon }_4=cos\frac{\theta }{2}$$
这四个参数满足：
$${\epsilon }_1^2+{\epsilon }_2^2+{\epsilon }_3^2+{\epsilon }_4^2=1$$

推导

说是推导，实际上就是说明几个符号的意义和算法的逻辑，真要看推导还得看参考文献1和3。
在这里，我们使用大写字母（比如X）表示矩阵，使用小写+斜体表示一个列向量,例如：$${\hat{\mathbf{x}}}_{\mathbf{k}-1|\mathbf{k}-1}和{\hat{\mathbf{x}}}_{\mathbf{k}|\mathbf{k}-1}$$
注意上面两个例子的下标和上标，\hat表示该值/向量为预测得到的，而k-1|k-1表示k-1时刻该值/向量为经过k-1时刻卡尔曼滤波计算后得到，k|k-1则表明这是利用k-1时刻对k时刻进行的预测（没有经过卡尔曼滤波）。

首先建立系统的状态空间方程（应该是这么说？）
$${\mathbf{x}}_{\mathbf{k}}=F{\mathbf{x}}_{\mathbf{k}-1}+\mathbf{B}{\mathbf{u}}_{\mathbf{k}}+{\mathbf{w}}_{\mathbf{k}}——(1)$$
$${\mathbf{z}}_{\mathbf{k}}=H{\mathbf{x}}_{\mathbf{k}}+{\mathbf{v}}_{\mathbf{k}}——(2)$$
式中：$${\mathbf{x}}_{\mathbf{k}}为k时刻系统的状态向量（同时也是你需要的滤波结果向量）$$
$${\mathbf{u}}_{\mathbf{k}}为k时刻系统的输入向量（也就是输入的控制量）,而{\mathbf{z}}_{\mathbf{k}}为观测$$
$$向量(一般与\mathbf{x}不同);{\mathbf{w}}_{\mathbf{k}}为过程噪声,{\mathbf{w}}_{\mathbf{k}}\sim N(0,{\mathbf{Q}}_{\mathbf{k}}),{\mathbf{Q}}_{\mathbf{k}}为过程噪$$
$$声协方差矩阵；{\mathbf{v}}_{\mathbf{k}}为观测噪声,{\mathbf{v}}_{\mathbf{k}}\sim N(0,{\mathbf{R}}_{\mathbf{k}}),{\mathbf{R}}_{\mathbf{k}}为观测噪声协$$
$$方差矩阵；F表示k-1时刻对k时刻的影响，B表示输入对状态的$$
$$影响;H将状态向量映射到观测向量空间，是一个线性映射。$$
基于（1）（2）式，我就开始推导(列公式)了。首先是预测过程，如果不考虑噪声，我们的预测是：
$${\hat{\mathbf{x}}}_{\mathbf{k}|\mathbf{k}-1}=F{\hat{\mathbf{x}}}_{\mathbf{k}-1|\mathbf{k}-1}+\mathbf{B}{\mathbf{u}}_{\mathbf{k}}——(3)$$
然后是对于协方差矩阵P进行预测，协方差矩阵P的定义是：
$${\mathbf{P}}_{\mathbf{k}}=Cov({\mathbf{x}}_{\mathbf{k}}-{\hat{\mathbf{x}}}_{\mathbf{k}|\mathbf{k}-1},{\mathbf{x}}_{\mathbf{k}}-{\hat{\mathbf{x}}}_{\mathbf{k}|\mathbf{k}-1})$$
对(1)和(3)做差，然后由于Q和预测无关，我们就可以得到对P的预测值:
$${\mathbf{P}}_{\mathbf{k}|\mathbf{k}-1}=F{\mathbf{P}}_{\mathbf{k}-1|\mathbf{k}-1}{F}^T+Q——(4)$$

对于MPU6050，我们设置观测变量为：
$${\mathbf{x}}_{\mathbf{k}}=\left[\begin{array}{c}\theta \\ {\dot{\theta }}_{bias}\end{array}\right]，{\dot{\theta }}_{bias}为角速度偏差值$$
显然，这两个是不相干的，因此协方差矩阵Q为一个对角阵，输入变量U为：
$${\mathbf{u}}_{\mathbf{k}}=\dot{\theta }，\dot{\theta }为角速度$$
那么矩阵F、B、Q为,dt为采样时间，Q_{θ}为角度的协方差：
$$\mathbf{F}=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right];\mathbf{B}=\left[\begin{array}{c}dt\\ 0\end{array}\right];\mathbf{Q}=\left[\begin{array}{cc}{Q}_{\theta }& 0\\ 0& Q_{{\dot{\theta }}_b}\end{array}\right]\cdot dt$$
关于协方差矩阵P，其初始化值其实不需要非常关心。因为如果是线性时不变系统，在经过一段时间后P会迭代到一个稳态的值，初值只是会改变这个迭代的速度。如果初始值是精确的，那么完全可以初始为一个0阵；对于我们这样设置的状态变量，因为其相互独立，所以P也是个对角阵，P可以初始化为,为一个比较大的值：
$$\mathbf{P}=\left[\begin{array}{cc}L& 0\\ 0& L\end{array}\right]$$
下面是观测：
根据（2）式，我们先来讨论一下向量和矩阵。
为了便于在C语言中实现，我们这里用的是二维的单独解算，如果我们设置观测向量为单纯某个角度（比如滚转角Roll），那么就不需要求矩阵的逆（具体原因后面说），大大简化运算。
因此，观测向量为一个角度标量，这个角度通过加速度算出；同样，观测噪声矩阵R也为一个标量。
$${\mathbf{z}}_{\mathbf{k}}=f(\mathbf{A}\mathbf{c}\mathbf{c}{\mathbf{x}}_{\mathbf{k}},\mathbf{A}\mathbf{c}\mathbf{c}{\mathbf{y}}_{\mathbf{k}},\mathbf{A}\mathbf{c}\mathbf{c}{\mathbf{z}}_{\mathbf{k}});\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]$$
关于R的值，R越大，证明我们对观测结果越不信任，会使得响应变慢；如果过小，那么噪声影响就会比较大。
观测我们先需要通过加速度获取观测值，这里用参考博文2 的atan2那个方法进行计算，具体可以看那篇博文。
然后计算预计和观测的误差,显然这个误差是个标量(scalar)：
$${\mathbf{y}}_{\mathbf{k}}={\mathbf{z}}_{\mathbf{k}}-H{\hat{\mathbf{x}}}_{\mathbf{k}|\mathbf{k}-1}$$
然后为了方便计算，我们将一个中间变量定义为矩阵S，在参考博文1中作者称其为innovation covariance，我也不知道是啥。
$$S=H{P}_{k|k-1}{H}^T+R$$
在我们这个情况下，这个矩阵S是一个标量，因此后面对其求逆时只需要做除法（填坑）。
下面进行算法的下一步：计算卡尔曼增益：
$$K_k=P_{k|k-1}{H}_T{S}^{-1}——(5)$$
$$对于我们具体实现，可以写为：K_k=\frac{P_{k|k-1}{H}^T}{[S]}$$
有了这一步的卡尔曼增益，我们就可以对预测和观测进行Fusion了：
$${\hat{\mathbf{x}}}_{\mathbf{k}|\mathbf{k}}={\hat{\mathbf{x}}}_{\mathbf{k}|\mathbf{k}-1}+K_k{\hat{\mathbf{y}}}_{\mathbf{k}}——(6)$$
最后一步就是实现协方差矩阵P的更新：
$${\mathbf{P}}_{\mathbf{k}|\mathbf{k}}=(I-K_{k}H){\mathbf{P}}_{\mathbf{k}|\mathbf{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

1. 卡尔曼滤波算法的推导以及C实现（这篇博文强烈推荐，说的特别详细）：a-practical-approach-to-kalman-filter-and-how-to-implement-it
2. 关于姿态角的加速度求解：静态条件下三轴加速度求角度的算法
3. 卡尔曼滤波算法的参数理解与推导？：卡尔曼滤波算法的参数理解
4. Goldman公式的推导：Goldman公式的推导
5. 机器人学导论[J.Craig]