前言

自己在课上吹的牛，课程作业再麻烦也得干。模了好几天鱼，终于在DDL前一天弄完了惯导模块的简单demo，卡尔曼滤波算是我弄的最久的了（大概2-3天），虽然没有彻底弄懂原理（概率论没学，隐马尔可夫链啥的更是不会），好歹就着教程推导给实现了。

实现的效果不咋的，还是存在明显的跳动，估计是MPU6050陀螺仪的事，方差大、数值基本是不准的，爬了。

理论推导

假定条件

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

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

欧拉角和四元数

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

欧拉角

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

那么我们就将绕Z轴旋转的角度Yaw称为偏航，绕X轴旋转的角度称为Roll滚转，绕Y轴旋转的角度Pitch称为俯仰角。

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

欧拉角实际上是一种转角排列设定法，转角的排列顺序比较多，但是很多旋转实际上是一样的。我们可以找到一个等效的旋转轴和对应这个轴的转角，那么我们假定K^是一个表示旋转轴的单位向量，RK(θ)为等效旋转矩阵\hat{K}是一个表示旋转轴的单位向量，R_{K}(θ)为等效旋转矩阵K^是一个表示旋转轴的单位向量，RK​(θ)为等效旋转矩阵

那么我们可以将被旋转向量V分解为沿轴分量V_c和V_1，利用向量叉积取垂直V_c和V_1的V_2。设旋转后的向量为V’,其在平面分量为V’_1，有：V⃗=Vc⃗+V1⃗\vec{V} = \vec{V_{c}}+\vec{V_{1}}V=Vc​​+V1​​

V′⃗=Vc⃗+V1′⃗\vec{V^{'}} =\vec{V_{c}}+\vec{V_{1}^{'}} V′=Vc​​+V1′​​

示意图（意思意思就行了（狗头））

下面就是用V_1和V_2表示旋转后的向量投影，假设转角为θ，θ实际上就是V’_1和V_1的夹角（图上忘画了），那么旋转后的向量就是：

V’⃗=Vc⃗+V1⃗cosθ+V2⃗sinθ\vec{V^{’}}=\vec{V_{c}}+\vec{V_{1}}cosθ+\vec{V_{2}}sinθV’=Vc​​+V1​​cosθ+V2​​sinθ

带入轴K的单位向量表示和各个坐标轴的坐标即可得到Goldman公式。例如：

K^=(Kx,Ky,Kz),Vc⃗=(Vc⃗⋅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轴，那么代入旋转后向量公式就可以求得矩阵的第一列。

最后的旋转矩阵应该是：

[Kx2(1−c)+cKxKy(1−c)−KzsKxKz(1−c)−KysKxKy(1−c)+KzsKy2(1−c)+cKyKz(1−c)−KxsKxKz(1−c)−KysKzKy(1−c)+KxsKz2(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=kxsinθ2\varepsilon_{1} = k_{x}sin\frac{\theta}{2}ε1​=kx​sin2θ​

ε2=kysinθ2\varepsilon_{2} = k_{y}sin\frac{\theta}{2}ε2​=ky​sin2θ​

ε3=kzsinθ2\varepsilon_{3} = k_{z}sin\frac{\theta}{2}ε3​=kz​sin2θ​

ε4=cosθ2\varepsilon_{4} = cos\frac{\theta}{2}ε4​=cos2θ​

这四个参数满足：

ε12+ε22+ε32+ε42=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时刻进行的预测（没有经过卡尔曼滤波）。

首先建立系统的状态空间方程（应该是这么说？）

xk=Fxk−1+Buk+wk——(1)\boldsymbol{{x}_{k}} = F\boldsymbol{{x}_{k-1}+B\boldsymbol{{u}_{k}}+\boldsymbol{{w}_{k}}} ——(1)xk​=Fxk−1​+Buk​+wk​——(1)

zk=Hxk+vk——(2)\boldsymbol{{z}_{k}} = H\boldsymbol{{x}_{k}}+\boldsymbol{{v}_{k}} ——(2) zk​=Hxk​+vk​——(2)

式中：xk为k时刻系统的状态向量（同时也是你需要的滤波结果向量）\boldsymbol{{x}_{k}}为k时刻系统的状态向量（同时也是你需要的滤波结果向量）xk​为k时刻系统的状态向量（同时也是你需要的滤波结果向量）

uk为k时刻系统的输入向量（也就是输入的控制量）,而zk为观测\boldsymbol{{u}_{k}}为k时刻系统的输入向量（也就是输入的控制量）,而\boldsymbol{{z}_{k}}为观测uk​为k时刻系统的输入向量（也就是输入的控制量）,而zk​为观测

向量(一般与x不同);wk为过程噪声,wk∼N(0,Qk),Qk为过程噪向量(一般与\boldsymbol{x}不同);\boldsymbol{{w}_{k}}为过程噪声,\boldsymbol{{w}_{k}}\sim N(0,\boldsymbol{Q_{k}}),\boldsymbol{Q_{k}}为过程噪向量(一般与x不同);wk​为过程噪声,wk​∼N(0,Qk​),Qk​为过程噪

声协方差矩阵；vk为观测噪声,vk∼N(0,Rk),Rk为观测噪声协声协方差矩阵；\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=Fx^k−1∣k−1+Buk——(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的定义是：

Pk=Cov(xk−x^k∣k−1,xk−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的预测值:

Pk∣k−1=FPk−1∣k−1FT+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，我们设置观测变量为：

xk=[θθ˙bias]，θ˙bias为角速度偏差值\boldsymbol{{x}_{k}} = \left[\begin{matrix} \theta \\ \dot{\theta}_{bias} \end{matrix}\right]， \dot{\theta}_{bias}为角速度偏差值xk​=[θθ˙bias​​]，θ˙bias​为角速度偏差值

显然，这两个是不相干的，因此协方差矩阵Q为一个对角阵，输入变量U为：

uk=θ˙，θ˙为角速度\boldsymbol{{u}_{k}} = \dot{\theta} ， \dot{\theta}为角速度uk​=θ˙，θ˙为角速度

那么矩阵F、B、Q为,dt为采样时间，Q_{θ}为角度的协方差：

F=[1dt01];B=[dt0];Q=[Qθ00Qθ˙b]⋅dt\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 dtF=[10​dt1​];B=[dt0​];Q=[Qθ​0​0Qθ˙b​​​]⋅dt

关于协方差矩阵P，其初始化值其实不需要非常关心。因为如果是线性时不变系统，在经过一段时间后P会迭代到一个稳态的值，初值只是会改变这个迭代的速度。如果初始值是精确的，那么完全可以初始为一个0阵；对于我们这样设置的状态变量，因为其相互独立，所以P也是个对角阵，P可以初始化为,为一个比较大的值：

P=[L00L]\boldsymbol{P}= \left[\begin{matrix} L &0 \\ 0 & L \end{matrix}\right]P=[L0​0L​]

下面是观测：

根据（2）式，我们先来讨论一下向量和矩阵。

为了便于在C语言中实现，我们这里用的是二维的单独解算，如果我们设置观测向量为单纯某个角度（比如滚转角Roll），那么就不需要求矩阵的逆（具体原因后面说），大大简化运算。

因此，观测向量为一个角度标量，这个角度通过加速度算出；同样，观测噪声矩阵R也为一个标量。

zk=f(Accxk,Accyk,Acczk);H=[10]\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)：

yk=zk−Hx^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=HPk∣k−1HT+RS = HP_{k|k-1}H^{T}+RS=HPk∣k−1​HT+R

在我们这个情况下，这个矩阵S是一个标量，因此后面对其求逆时只需要做除法（填坑）。

下面进行算法的下一步：计算卡尔曼增益：

Kk=Pk∣k−1HTS−1——(5)K_{k} = P_{k|k-1}H_{T}S^{-1}——(5)Kk​=Pk∣k−1​HT​S−1——(5)

对于我们具体实现，可以写为：Kk=Pk∣k−1HT[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+Kky^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的更新：

Pk∣k=(I−KkH)Pk∣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 npimport matplotlib.pyplot as pltimport math#compute the sigma of accel and gryo's measurementdataset = 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 = 0GAngleList = []#预测得到的角度AAngleList = []#观测值得到的角度KAngleList = []#Kalman滤波后的角度KalmanList = []#Kalman 增益值1KalmanList2 = []#Kalman 增益值2BiasList = []angle = 0#初始角度为0bias = 0 #初始的角速度偏差为0#参量：dt采样时间，P初始协方差矩阵（与收敛有关），Q过程噪声矩阵（越大预测可信度越低），测量噪声矩阵R（R越大观测可信度越低）dt = 0.5X = np.array([[angle],[bias]]) #预测变量Gryo = 0 #输入控制变量Z = 0 #观测量，为加速度计算出的角度.实际上，加速度计算出的角度，只能说铅锤轴的加速度g不变推导出的Y =0#观测量与预计值的误差，Y = Z-HXP = 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，角速度为测量得到的方差，实际上真正的过程噪声矩阵还应该xdtR = 0.01 #测量噪声矩阵，由于是二维且只有角度，因此转换为标量，使用加速度的方差F = np.array([[1,-dt],[0,1]])B = np.array([[dt],[0]])#输入作用于预测的矩阵BH= 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*dtGAngleList.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))计算,网上的欧拉角计算有两种方式，#参考/qcopter/article/details/51848544Z = RollAAngleList.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]

如果觉得《MPU6050卡尔曼滤波解算姿态角》对你有帮助，请点赞、收藏，并留下你的观点哦！