我又消失了一段时间，这段时间研究了惯性导航有关的算法，整理了不少博客，字数比较多，图片比较多。学到了很多知识。
目录
前言
本节介绍
一、Mahony算法
1.1 PID控制算法
1.2 模糊PID
1.3 Mahony算法
1.4 互补滤波的思考
二、卡尔曼滤波
2.1 以前的卡尔曼滤波
2.2 公式推导
2.2.1 文字叙述
2.2.2 代码
这一节主要介绍关于IMU相关算法的阅读与思索，准确的说是介绍Mahony算法和卡尔曼滤波算法
在叙述mahony算法之前，先说一下最经典的PID控制算法。
这个算法接触的很长了，但是一直没有考虑在STM32上是如何运转的。这位博客大佬PID具体原理说的很好，就不搬运了
(65条消息) PID超详细教程——PID原理+串级PID+C代码+在线仿真调参_skythinker616的博客-CSDN博客
不过还是要简单说一下PID的控制步骤：
比例P：提供反方向力产生加速度，迫使反向方向运动，但是往往这种力不会平衡，甚至有可能永远震动下去。
积分I: 主要是平衡干扰力的影响，达到预期的平衡点。
微分D：利用一阶积分实现达到预计平衡的作用，但是一般难以实现设计。
上面博主介绍的非常仔细，如果看完了可以直接考虑物理量的设计这一环节，也就是：
目标值、反馈值、输出值的考虑
我们的想法是让MPU输出有效的陀螺仪还有加速度计的数据，但是这个距离我们设计的步骤还有一些遥远。
比方说：虽然我们在上节的介绍通过IIC能够printf打印6050直接得到了6个数据，我们知道这些数据是不准的，但是在PID来滤波，怎么得到真实值？也就是说我们让这6组数据达到什么效果？PID的反馈值如何设定？
其实：PID控制的时候并不是直接控制这六个参数，而是根据这六个数据输出位姿，控制位姿。此外，目前市面上大多都是采用PID来控制小车，所以在回馈量上都是关于小车的量，纯惯导与这个没有多大的的关系，对于主流的小车PID差速控制之类的就不做过多的研究。
经过一段时间的调研和查询，发现实际做惯导的时候并不是仅仅使用PID控制的（或者说用PID控制的比较少），而是使用mahony算法，还有卡尔曼滤波算法。
1.2 模糊PID
2023/4/14补充模糊PID
感谢博客(80条消息) 【智能车】模糊PID控制原理详解与代码实现_Ethan-Code的博客-CSDN博客
普通PID的参数Kp以及Ki都是要自己不断调节的。可以说换了一个环境就需要调一次。不过有一种PID的参数是可以自己变化，那就是模糊PID了（虽然他自己调过程的比一般PID要更多了）。一般这种方法用在四驱小车中，我同门和我说这种方法用在惯导设备中的效果其实并不是很好。谁知道呢？就像基于深度学习的PID（自己学Kp，Ki）。
回归正题：模糊PID一般要求回馈e需要经过求导阶段
我一开始看以为是自己设定多个范围，e只要在那个范围Kp还有Ki，就会自己设定这个数值；虽然和我理解的差不多吧，但是实际更加的复杂。
过程比较复杂，这里最简单的叙述一下：
首先将e设定一个固定范围（只有在这个范围才有效），然后划分六个区间：（左图所示）。图中设定了（-3——3），属于这个区间的e按照固定范围就会转为（-3，3）之间的数值。
然后就牵扯到一个隶属度函数（注意：0-1之间），我理解：每个范围PID参数不是没有作用，只是在其他小区间中影响的就小了。这种影响有三角形、矩形、抛物线形式（中间所示的就是三角形模式）
由e确定在（-3，3）之间的数值就会对应这个隶属度函数中的数值（右图所示），在途中对应的数值是0.6的PM，0.4的PB.
e求导也是按照这样一个过程。也能得到一组隶属度（类似0.6的PM，0.4的PB的表达）
接下来就是重点了：
e和e的导数隶属度两两组合，根据两个隶属度来查表
该博客就给了一个例子：
结果Kp求出就是
(65条消息) 电赛四轴小结——姿态解算篇【mahony互补滤波算法】_linzs.online的博客-CSDN博客
上面博客介绍的Mahony算法大意是这样的：
加速度计会给出三组数据，这三组数据在IMU模块（DMP可能会固件滤波一下）是直接使用的。但是考虑到误差，一般通过平衡时候的重力加速度[0 0 g]T通过计算求出位姿变化矩阵中的参数（但求不出航向角）。
陀螺仪的加入则是弥补了加速度计的这种不确定性。陀螺仪解决的是：角度变化与时间的比，单位：度/秒，如果说世界是理想的、只需要对3个轴的陀螺仪角速度进行积分，得到3个方向上的旋转角度，也就是姿态数据。
磁力计的加入主要是实现航向角的输出。不过做惯导一般不考虑航向角。
在理想世界，陀螺仪和加速度计都能实现输出位姿。实际上陀螺仪解算得到的姿态具有良好的高频特性，但是会随着时间漂移，而加速度计解算得到的姿态具有良好的低频特性，不会随着时间漂移，但是载体剧烈运动时，往往不能解算出真实的姿态。所以我们可以将陀螺仪的高频特性和加速度计的低频特性相融合，得到高频、低频特性都很好的算法。
这就是mahony算法的核心点——数据融合（互补滤波）
直白一点的说：融合加速度计还有陀螺仪数据进行位姿估计，也就是博客说的：
-加速度计告诉我们：“你现在的位置是Racc”
-我们回答：“谢谢，但让我确认一下”
-然后根据陀螺仪的数据和上一次的自己计算出来的准确值修正这个值并输出新的估算值。
接下来就是进一步的公式推导步骤了，建议参考贴吧：
Mahony姿态解算算法笔记（一） - 知乎 (zhihu.com)
Mahony姿态解算算法笔记（二） - 知乎 (zhihu.com)
该博客推导过程挺好理解的:
最后该算法还是用到了PID控制。
当然，如果你想加入磁力计，获取航向角，建议看看这篇博客
[知识点]基于加速度计与磁力计的姿态解算方法(加计补偿偏航) - 知乎 (zhihu.com)
2023/4/14补充：该贴吧还介绍了一个比较重要的内容（同时也是下面需要介绍的）：该贴吧谈到：
首先，对于六轴数据，计算角度有两种方法，一种是通过对角速度积分得到角度，另一种则是通过对加速度进行正交分解得到角度。但这两种方式均存在不足，通过角速度积分得到角度时，角速度的误差会在积分过程中被不断放大从而影响数据准确性。而加速度计是一种特别敏感的传感器，电机旋转产生的震动会给加速度计的数据中带来高频噪声。
不难看出，第一种方法测得的数据中存在结果偏差，而第二种方法测得的数据中存在高频噪声。因此可通过融合两种数据以获得准确姿态，这里通过加速度计补偿角速度。
2023/4/14补充
感谢博客（补充了后面的位姿递推）：(80条消息) Mahony姿态解算算法详解_mahony算法_白鸟无言的博客-CSDN博客
上述Mahony算法还存在一个很大的误区点：
说透互补滤波(1) - 线性互补滤波器从原理到实现 - 知乎 (zhihu.com)
Mahony算法的核心在于：加速度计带有低频噪声，陀螺仪带有高频噪声（也可以理解为加速度信号对高频敏感，陀螺仪信号对低频敏感，不敏感效果就差了呗），所以我们把他们的噪声分别滤除，然后合并，就得到了没有噪声的原始信号。
但是实际你会发现两组数据其实在结算的时候只能使用一组：平衡情况下使用[0 0 g]T实现位姿求取，但是陀螺仪只有在发生转动的时候才能有数据，所以陀螺仪无法提供初始状态angle0。更可怕的是不平衡情况下（也就是运动情况下）只有陀螺仪一组数据能用，加速度计完全用不了。
就像贴吧说的那样：连两路输入都没有，还怎么互补滤波？所以在使用的互补滤波概念的时候是做了一个假设（参考上述博客）：
之后关于低通滤波还有高通滤波这里就不细说了，贴吧还详细介绍了一、二阶低通滤波的原理，这部分估计要下次研究看到具体代码才能研究透彻了。（待续......）
在上课的时候经常看到卡尔曼滤波的公式，尤其是考试的时候背的都烂了，但是在应用的时候还是不太会，趁这个时间，准备好好理解一下。下图是在课上经常看到的卡尔曼滤波五个公式：
在IMU实际应用解算的时候，会发现和上面公式多了几步或者有些出入。但大体是差不多了。
接下来就是阐述卡尔曼滤波的过程
（图片有些多，内容来源TKJ电子 » 卡尔曼滤波的实用方法及其实现方法 (tkjelectronics.dk)）
让你直接看推导你是肯定接受不了的，所以要先文字叙述一下，你才能接受。由于公式编辑的麻烦，这部分我先在word中编译之后在贴图展示：
后面推导就需要参考深度解析卡尔曼滤波在IMU中的使用 - 知乎 (zhihu.com)了，直接给出相关的代码：
/*************卡尔曼滤波*********************************/
void Kalman_Filter(float Accel,float Gyro)
{
static const float Q_angle=0.001;
static const float Q_gyro=0.003;
static const float R_angle=0.5;
static const float dt=0.01; //dt为kalman滤波器采样时间;
static const char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={0,0,0,0};
static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
Angle+=(Gyro - Q_bias) * dt; //1角的先验估计 x_k=Fx_(k-1)+Bu_k+w_k，
//2计算误差协方差矩 P_{k|k-1}=FP_{k-1|k-1}F^T+Q_k
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - Angle; //3更新 y_k=z_k-Hx_{k|k-1}
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0; //4 更新协方差 S_{k}=HP_{k|k-1}H^T+R
K_0 = PCt_0 / E; //5 计算卡尔曼增益 K_k=P_{k|k-1}H^TS_k^{-1}
K_1 = PCt_1 / E;
// 6 获取后验估计x_{k|k}=x_{k|k-1}+K_k*y_k
//注意这个其实是向量x_{k|k}拆开成两部分了，一是角度，另一个是角速率(这个角速率很有用从陀螺仪测量中减去偏差来获得真实速率)
Angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
// 7 更新后验误差协方差矩阵
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
void Angle_Calculate(void)
{
static uint8_t DataBuffer[2]; //数据缓存
/****************************加速度****************************************/
I2C_ReadBuffer(DataBuffer, ACCEL_XOUT_H, 2);
Accel_x = (short)((DataBuffer[0]<<8)+DataBuffer[1]); //读取X轴加速度
Angle_ax = (Accel_x +220) /16384; //去除零点偏移,计算得到角度（弧度）
Angle_ax = Angle_ax*1.2*180/3.14; //弧度转换为度,
/****************************角速度****************************************/
I2C_ReadBuffer(DataBuffer, GYRO_YOUT_H, 2);
Gyro_y = (short)((DataBuffer[0]<<8)+DataBuffer[1]); //静止时角速度Y轴输出为-18左右
Gyro_y = (Gyro_y + 18)/16.4; //去除零点偏移，计算角速度值
// Angle_gy = Angle_gy + Gyro_y*0.01; //角速度积分得到倾斜角度.
/***************************卡尔曼滤波+角度融合*************************************/
Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
/*******************************互补滤波******************************************/
/*补偿原理是取当前倾角和加速度获
得倾角差值进行放大，然后与陀螺
仪角速度叠加后再积分，从而使倾
角最跟踪为加速度获得的角度0.5
为放大倍数，可调节补偿度;
0.01为系统周期10ms
*/
// Angle = Angle + (((Angle_ax-Angle)*0.5 + Gyro_y)*0.01);*/
}