前文讲到APM的三阶互补方案，之前附的图是从学长博客里面抠的，感觉还不是很详细，于是自己就画了下，顺便重新理一下思路。
上图中下标为O的表示原始量（Origion），C表示矫正后的量（Correction），a，v，s这些一目了然，表示加速度、速度、位置，其中带下标c的表示融合后的状态量，即在控制中用作实际反馈的量，读者可以结合上图与上文博客后面所贴的相关代码、注释阅读。
链接如下：
四旋翼定高篇之惯导加速度+速度+位置三阶互补融合方案
上文最后提到由于观测传感器滞后性（主要原因：1、支持的最大采样频率小；2、原始数据输出噪声大、大多数时候需要数字低通滤波造成时延），造成的直接把当前惯导估计值与观测传感器做差比较得到状态误差的方式不可取。
在提出解决办法前，首先我们来谈下传感器采样周期造成的观测传感器滞后的问题：
首先以大家熟悉的超声波传感器为例，其中辨识度最高的为HC-SR04
直接给出采集时序图：
超声波传感器工作需要模块发射头向外发射波长约为6mm、频率为40khz的超声波信号，触发模块开始工作需要在模块触发引脚产生一个不少于10us的高电平，取触发时刻为T0，当前方有物体时，声波会反射回来，反射回来的信号被模块上的接收头所接收，并且在模块的输出引脚上产生一个回响应电平信号，取响应时刻回T1，根据声波一来一往的总时间与声音在空气中传播的速度，即可计算距前方物体距离。
此模块为了防止发射信号对回响信号的影响，需要两次发射间隔至少为60ms，模块测量距离范围为2cm~400cm，实际使用过程中，也就发现2.5m以内数据还好，超过2.5m后，稍微有点角度误差就很大，我们就按照最大检测距离为2.5m计算，最大采集时间为60ms+（2.5*2/340）*1000ms=74.7ms
至此我们总结下，常见的HC-SR04模块最小采样周期为60ms，最
大接近75ms。
75ms似乎完全可以接受，下面再以常用的M8N GPS为例：
直接截取ublox软件设置截图：
这里首先可以看到GPS时钟源，一般就选GPS time即可，接着是
测量周期，一般模块初始化为1Hz，对于一般的应用来讲，1Hz刷新
速率完全够了，这也是模块默认的刷新速率。更快的刷新速率意
味着需要占用芯片的更大通讯资源、处理压力，同时模块的功耗
也就越大。
（尚不了解刷新速率加大是否会对数据输出精度有影响。。。）
本文当前采用的不是串口解析Nema标准字符数据帧形式，本文只
对UBX里面PVT语句进行解析，PVT数据帧信息如下图所示：
PVT语句基本上包含了GPS定点所需的所有常用信息，本文直接
GPS测量频率设置成M8N所能允许的最大采样频率10Hz后，此时
的GPS采样时间即为100ms。
下面接着讨论常用高度观测传感器———气压计MS5611，数据
手册截图如下：
其中气压计采集过程为气压、温度交替采集，采样频率可设置，
采集时，需要先开启ADC（气压、温度）转换，然后采集，这里
的Responce Time表示的为开启到采样的所需间隔时间。一般为
保证压力数据、温度数据的实时同步性，在采集温度前、开启气
压ADC数据转换，同理，采集气压前，亦开启温度ADC转换。这
样可以实现采样周期的最小化。
从MS5611数据手册可以很容易的知道温度+气压采样周期最小为
0.5*2=1ms，最大为8.22*2=16.44ms，这里需要注意的是不同采
样频率下，传感器的输出精度是不一样的，从
0.012mbar到0.065mbar，其中当采样周期为16.44ms时，气压
误差为0.012mbar，采样周期为1.0ms时，气压误差为
0.065mbar，这里出现的mbar表示气压单位毫帕。单位转换关系如
下图：
故0.0012mbar=1.20Pa，有常识可知1Pa误差约等于10cm，将气
压误差转换为距离后误差在12cm~65cm。更小采样的采样周期
意味着更大的采样误差，不同的噪声误差对应着卡尔曼滤波时观
测噪声的选取大小。
这里我们参考Autoquad飞控里面的采样设置，直接抠图如下：
这里可以看到，传感器ADC转换时，温度与气压均设置成4096，
即此时最小采样周期为16.44ms，在AQ飞控CoOS任务调度周期为
2.5ms，意味着只能实现最小20ms的气压计采样周期。（其中缘
由大家自己算下加法即可）。
最后在介绍一种位置观测传感器：光流PX4FLOW
下面一段话为官网PX4FLOW中文介绍截取部分：
PX4Flow 是一款智能光学流动传感器。传感器拥有原生752×480 像素分辨率，计算光
学流的过程中采用了4倍分级和剪裁算法，计算速度达到250Hz（白天，室外），具备
非常高的感光度。与其他滑鼠传感器不同，它可以以 120Hz（黑暗，室内）的计算速度
在室内或者室外暗光环境下工作，而无需照明LED。你也可以对它重新编程，用于执行
其他基础的，高效率的低等级机器视 觉任务。
PX4FLOW支持USB、串口、I2C两种方式对数据进行解析，为了减小芯片开销
作者采用的方式为I2C形式，其中I2C相关数据为如下图：
其中包含：光流点数、光流积分量、结合高度转换后的光流速度、超声波距离、图像
质量、三轴角速度、超声波测量时间、陀螺仪内部温度等数据。
其中：光流速度、超声波距离、图像质量为定点、定高所需的有用数据，作者在这里只
对这三组数据进行了获取。
PX4FLOW主控采用的是STM32F405，芯片主要资源开销为DCMI获取摄像头数据与处理
+融合高度、陀螺仪光流算法（其余超声波数据采集、板载陀螺仪SPI数据采集、
Mavlink、USB等基本上可以忽略）。
这里PX4FLOW给出了最大处理速度时耗时4ms，尚不清楚这部分指的是处理完一场图
像数据加融合的总开销，还是只是针对融合算法。
作者主控采用的是STM32F103RCT6，采用模拟I2C采集PX4FLOW数据，为保证陀螺仪
、加计采样以及控制周期最小化，在采集I2C时，对PX4FLOW采用的是队列采集模式，
即第一个2ms采集X轴流速、第二个2ms采集Y轴流速、第三个2ms采集超声波距离、第
四个2ms采集图像质量，光流数据更新一次为8ms。
至此我们列举几种常见传感器采样周期如
下：
超声波HC-SR04：75ms
GPS M8N:100ms
气压计MS5611：20ms
PX4FLOW:8ms
上述我们只是讨论了数字传感器采集过程中的传感器采样周期造成的时延。
这里我们讨论的时延是相对于惯性导航主导传感器——加速度计而言的。
MPU6500加速度计当不设置内部数字低通时，陀螺仪最大输出频率为8Khz，加速度计为
4Khz，对于四旋翼而言，最大采样频率为1Khz就完全够用了，足够高的采样周期能减小
数据融合时的积分误差（频率混叠可以忽略），同时保证传感器数据数字低通时的群时
延也更小。
作者由于STM32F103RCT6芯片运算速度限制，MEMS传感器组合为MPU6500、HMC5983、
MS5611，这三组传感器数据通过SPI采集进来分别耗时：73us、56us、82us。
姿态解算最大耗时约：320us
GPS串口解析最大耗时约：200us
PX4FLOW单个数据采集最大耗时约：0.5ms
三轴惯导卡尔曼融合最大耗时约：270us
控制器、数字滤波等最大耗时约：150us
系统总时间开销约为1.65ms
作者选取总定时调度周期为2.0ms
上面我们考虑的是一类原始数据采集过程中的滞后，接下来介绍一类因数字低
通滤波器造成的传感器时延问题。
传感器数字低通的滞后性：
首先以气压计传感器为例子，上文讲到当MS5611气压传感器设置成精度最高时，即开启
ADC采集时，温度、气压转换都设置成4096，此时官方给出的传感器误差为12cm，Okay，
现在我们先来一组原始气压通过压差法获取的相对高度值波形。
上图中，中间蓝色线表示速度波形、灰色表示加速度波形、上面总共有三条线，放大后
如下：噪声比较大的为原始气压通过压差换算高度值、平滑点的红色线表示惯导估计高
度，较为滞后的绿色线表示，巴特沃斯2Hz截止频率后的滤波滞后的高度值。
这里注意到，气压计原始高度波动比较大，高度噪声基本上在50cm以内，数字低通后的
气压高度即波动比较小，不考虑传感器静止漂移情况下，短时间内波动在15cm以内，但
是运动起来后，发现滞后性很明显。（曲线从一定高度至另一高度的跟踪出现明显
先后）
如果觉得还不够明显，当速度快一点后，就立马一目了然了。
注意速度峰值大的地方，惯导位置估计高度与低通后的气压观测高度，明显都是红色线
先起来、观测传感器后起来，中间来回上下拖动飞机一段更为明显。后面最后一个速度
峰值较小时，两曲线基本重合，看不出明显差异。
下面给出利用MATLAB设计巴特沃斯低通滤波器过程（Tip：作者信号与系统很渣，只会依葫
芦画瓢）：
首先本文巴特沃斯滤波器为2阶，阶数越高，虽然保证了阻带更快衰减，但是系统相
延亦增长。
1、调用MATLAB滤波器设计工具箱，命令行输入fdatool即可，界面如下。
2、左下方分别勾选Lowpass、IIR、滤波器阶数、采样频率、截止频率即可。
3、点击Design Filter滤波器设计完成，得到系统幅频响应如下。
4、点击Analysis，选Phase Delay即可看到系统相延特性。
上面两个图的差异在于设置的滤波器阶次不一样，对比可知，阶次越高系统相延越大。
二阶巴特沃斯数字低通滤波器参数如上图所示。
通过fdatool滤波器工具箱导出的滤波器参数，由巴特沃斯数字滤波器了离散方程：
数字控制器设计时，根据实时采样的加速度计数据，递推更新即可。
/*************************************************
函数名： LPButterworth（float curr_input，Butter_BufferData *Buffer，Butter_Parameter *Parameter）
说明： 二阶巴特沃斯数字低通滤波器
入口： float curr_input 当前输入
出口： 滤波器输出值
备注： 2阶Butterworth低通滤波器
*************************************************/
float LPButterworth（float curr_input，Butter_BufferData *Buffer，Butter_Parameter *Parameter）
{
static int LPB_Cnt=0;
/* 加速度计Butterworth滤波 */
/* 获取最新x（n） */
Buffer-》Input_Butter［2］=curr_input;
if（LPB_Cnt》=500）
{
/* Butterworth滤波 */
Buffer-》Output_Butter［2］=
Parameter-》b［0］ * Buffer-》Input_Butter［2］
+Parameter-》b［1］ * Buffer-》Input_Butter［1］
+Parameter-》b［2］ * Buffer-》Input_Butter［0］
-Parameter-》a［1］ * Buffer-》Output_Butter［1］
-Parameter-》a［2］ * Buffer-》Output_Butter［0］;
}
else
{
Buffer-》Output_Butter［2］=Buffer-》Input_Butter［2］;
LPB_Cnt++;
}
/* x（n） 序列保存 */
Buffer-》Input_Butter［0］=Buffer-》Input_Butter［1］;
Buffer-》Input_Butter［1］=Buffer-》Input_Butter［2］;
/* y（n） 序列保存 */
Buffer-》Output_Butter［0］=Buffer-》Output_Butter［1］;
Buffer-》Output_Butter［1］=Buffer-》Output_Butter［2］;
return （Buffer-》Output_Butter［2］）;
}
相关结构体定义如下。
typedef struct
{
//volatile
float Input_Butter［3］;
//volatile
float Output_Butter［3］;
}Butter_BufferData;
typedef struct
{
float a［3］;
float b［3］;
}Butter_Parameter;
自研飞控视频，链接如下：
四旋翼GPS户外定点篇（F330机架）
四旋翼GPS定点F450青山江边
此篇博客废话太多，疏于整理，难免有不正之处，欢迎交流指正！！！
下节讨论传感器延时修正的简单处理方法与惯性导航数据融合的细节部分。。。
前文讲到APM的三阶互补方案，之前附的图是从学长博客里面抠的，感觉还不是很详细，于是自己就画了下，顺便重新理一下思路。
上图中下标为O的表示原始量（Origion），C表示矫正后的量（Correction），a，v，s这些一目了然，表示加速度、速度、位置，其中带下标c的表示融合后的状态量，即在控制中用作实际反馈的量，读者可以结合上图与上文博客后面所贴的相关代码、注释阅读。
链接如下：
四旋翼定高篇之惯导加速度+速度+位置三阶互补融合方案
上文最后提到由于观测传感器滞后性（主要原因：1、支持的最大采样频率小；2、原始数据输出噪声大、大多数时候需要数字低通滤波造成时延），造成的直接把当前惯导估计值与观测传感器做差比较得到状态误差的方式不可取。
在提出解决办法前，首先我们来谈下传感器采样周期造成的观测传感器滞后的问题：
首先以大家熟悉的超声波传感器为例，其中辨识度最高的为HC-SR04
直接给出采集时序图：
超声波传感器工作需要模块发射头向外发射波长约为6mm、频率为40khz的超声波信号，触发模块开始工作需要在模块触发引脚产生一个不少于10us的高电平，取触发时刻为T0，当前方有物体时，声波会反射回来，反射回来的信号被模块上的接收头所接收，并且在模块的输出引脚上产生一个回响应电平信号，取响应时刻回T1，根据声波一来一往的总时间与声音在空气中传播的速度，即可计算距前方物体距离。
此模块为了防止发射信号对回响信号的影响，需要两次发射间隔至少为60ms，模块测量距离范围为2cm~400cm，实际使用过程中，也就发现2.5m以内数据还好，超过2.5m后，稍微有点角度误差就很大，我们就按照最大检测距离为2.5m计算，最大采集时间为60ms+（2.5*2/340）*1000ms=74.7ms
至此我们总结下，常见的HC-SR04模块最小采样周期为60ms，最
大接近75ms。
75ms似乎完全可以接受，下面再以常用的M8N GPS为例：
直接截取ublox软件设置截图：
这里首先可以看到GPS时钟源，一般就选GPS time即可，接着是
测量周期，一般模块初始化为1Hz，对于一般的应用来讲，1Hz刷新
速率完全够了，这也是模块默认的刷新速率。更快的刷新速率意
味着需要占用芯片的更大通讯资源、处理压力，同时模块的功耗
也就越大。
（尚不了解刷新速率加大是否会对数据输出精度有影响。。。）
本文当前采用的不是串口解析Nema标准字符数据帧形式，本文只
对UBX里面PVT语句进行解析，PVT数据帧信息如下图所示：
PVT语句基本上包含了GPS定点所需的所有常用信息，本文直接
GPS测量频率设置成M8N所能允许的最大采样频率10Hz后，此时
的GPS采样时间即为100ms。
下面接着讨论常用高度观测传感器———气压计MS5611，数据
手册截图如下：
其中气压计采集过程为气压、温度交替采集，采样频率可设置，
采集时，需要先开启ADC（气压、温度）转换，然后采集，这里
的Responce Time表示的为开启到采样的所需间隔时间。一般为
保证压力数据、温度数据的实时同步性，在采集温度前、开启气
压ADC数据转换，同理，采集气压前，亦开启温度ADC转换。这
样可以实现采样周期的最小化。
从MS5611数据手册可以很容易的知道温度+气压采样周期最小为
0.5*2=1ms，最大为8.22*2=16.44ms，这里需要注意的是不同采
样频率下，传感器的输出精度是不一样的，从
0.012mbar到0.065mbar，其中当采样周期为16.44ms时，气压
误差为0.012mbar，采样周期为1.0ms时，气压误差为
0.065mbar，这里出现的mbar表示气压单位毫帕。单位转换关系如
下图：
故0.0012mbar=1.20Pa，有常识可知1Pa误差约等于10cm，将气
压误差转换为距离后误差在12cm~65cm。更小采样的采样周期
意味着更大的采样误差，不同的噪声误差对应着卡尔曼滤波时观
测噪声的选取大小。
这里我们参考Autoquad飞控里面的采样设置，直接抠图如下：
这里可以看到，传感器ADC转换时，温度与气压均设置成4096，
即此时最小采样周期为16.44ms，在AQ飞控CoOS任务调度周期为
2.5ms，意味着只能实现最小20ms的气压计采样周期。（其中缘
由大家自己算下加法即可）。
最后在介绍一种位置观测传感器：光流PX4FLOW
下面一段话为官网PX4FLOW中文介绍截取部分：
PX4Flow 是一款智能光学流动传感器。传感器拥有原生752×480 像素分辨率，计算光
学流的过程中采用了4倍分级和剪裁算法，计算速度达到250Hz（白天，室外），具备
非常高的感光度。与其他滑鼠传感器不同，它可以以 120Hz（黑暗，室内）的计算速度
在室内或者室外暗光环境下工作，而无需照明LED。你也可以对它重新编程，用于执行
其他基础的，高效率的低等级机器视 觉任务。
PX4FLOW支持USB、串口、I2C两种方式对数据进行解析，为了减小芯片开销
作者采用的方式为I2C形式，其中I2C相关数据为如下图：
其中包含：光流点数、光流积分量、结合高度转换后的光流速度、超声波距离、图像
质量、三轴角速度、超声波测量时间、陀螺仪内部温度等数据。
其中：光流速度、超声波距离、图像质量为定点、定高所需的有用数据，作者在这里只
对这三组数据进行了获取。
PX4FLOW主控采用的是STM32F405，芯片主要资源开销为DCMI获取摄像头数据与处理
+融合高度、陀螺仪光流算法（其余超声波数据采集、板载陀螺仪SPI数据采集、
Mavlink、USB等基本上可以忽略）。
这里PX4FLOW给出了最大处理速度时耗时4ms，尚不清楚这部分指的是处理完一场图
像数据加融合的总开销，还是只是针对融合算法。
作者主控采用的是STM32F103RCT6，采用模拟I2C采集PX4FLOW数据，为保证陀螺仪
、加计采样以及控制周期最小化，在采集I2C时，对PX4FLOW采用的是队列采集模式，
即第一个2ms采集X轴流速、第二个2ms采集Y轴流速、第三个2ms采集超声波距离、第
四个2ms采集图像质量，光流数据更新一次为8ms。
至此我们列举几种常见传感器采样周期如
下：
超声波HC-SR04：75ms
GPS M8N:100ms
气压计MS5611：20ms
PX4FLOW:8ms
上述我们只是讨论了数字传感器采集过程中的传感器采样周期造成的时延。
这里我们讨论的时延是相对于惯性导航主导传感器——加速度计而言的。
MPU6500加速度计当不设置内部数字低通时，陀螺仪最大输出频率为8Khz，加速度计为
4Khz，对于四旋翼而言，最大采样频率为1Khz就完全够用了，足够高的采样周期能减小
数据融合时的积分误差（频率混叠可以忽略），同时保证传感器数据数字低通时的群时
延也更小。
作者由于STM32F103RCT6芯片运算速度限制，MEMS传感器组合为MPU6500、HMC5983、
MS5611，这三组传感器数据通过SPI采集进来分别耗时：73us、56us、82us。
姿态解算最大耗时约：320us
GPS串口解析最大耗时约：200us
PX4FLOW单个数据采集最大耗时约：0.5ms
三轴惯导卡尔曼融合最大耗时约：270us
控制器、数字滤波等最大耗时约：150us
系统总时间开销约为1.65ms
作者选取总定时调度周期为2.0ms
上面我们考虑的是一类原始数据采集过程中的滞后，接下来介绍一类因数字低
通滤波器造成的传感器时延问题。
传感器数字低通的滞后性：
首先以气压计传感器为例子，上文讲到当MS5611气压传感器设置成精度最高时，即开启
ADC采集时，温度、气压转换都设置成4096，此时官方给出的传感器误差为12cm，Okay，
现在我们先来一组原始气压通过压差法获取的相对高度值波形。
上图中，中间蓝色线表示速度波形、灰色表示加速度波形、上面总共有三条线，放大后
如下：噪声比较大的为原始气压通过压差换算高度值、平滑点的红色线表示惯导估计高
度，较为滞后的绿色线表示，巴特沃斯2Hz截止频率后的滤波滞后的高度值。
这里注意到，气压计原始高度波动比较大，高度噪声基本上在50cm以内，数字低通后的
气压高度即波动比较小，不考虑传感器静止漂移情况下，短时间内波动在15cm以内，但
是运动起来后，发现滞后性很明显。（曲线从一定高度至另一高度的跟踪出现明显
先后）
如果觉得还不够明显，当速度快一点后，就立马一目了然了。
注意速度峰值大的地方，惯导位置估计高度与低通后的气压观测高度，明显都是红色线
先起来、观测传感器后起来，中间来回上下拖动飞机一段更为明显。后面最后一个速度
峰值较小时，两曲线基本重合，看不出明显差异。
下面给出利用MATLAB设计巴特沃斯低通滤波器过程（Tip：作者信号与系统很渣，只会依葫
芦画瓢）：
首先本文巴特沃斯滤波器为2阶，阶数越高，虽然保证了阻带更快衰减，但是系统相
延亦增长。
1、调用MATLAB滤波器设计工具箱，命令行输入fdatool即可，界面如下。
2、左下方分别勾选Lowpass、IIR、滤波器阶数、采样频率、截止频率即可。
3、点击Design Filter滤波器设计完成，得到系统幅频响应如下。
4、点击Analysis，选Phase Delay即可看到系统相延特性。
上面两个图的差异在于设置的滤波器阶次不一样，对比可知，阶次越高系统相延越大。
二阶巴特沃斯数字低通滤波器参数如上图所示。
通过fdatool滤波器工具箱导出的滤波器参数，由巴特沃斯数字滤波器了离散方程：
数字控制器设计时，根据实时采样的加速度计数据，递推更新即可。
/*************************************************
函数名： LPButterworth（float curr_input，Butter_BufferData *Buffer，Butter_Parameter *Parameter）
说明： 二阶巴特沃斯数字低通滤波器
入口： float curr_input 当前输入
出口： 滤波器输出值
备注： 2阶Butterworth低通滤波器
*************************************************/
float LPButterworth（float curr_input，Butter_BufferData *Buffer，Butter_Parameter *Parameter）
{
static int LPB_Cnt=0;
/* 加速度计Butterworth滤波 */
/* 获取最新x（n） */
Buffer-》Input_Butter［2］=curr_input;
if（LPB_Cnt》=500）
{
/* Butterworth滤波 */
Buffer-》Output_Butter［2］=
Parameter-》b［0］ * Buffer-》Input_Butter［2］
+Parameter-》b［1］ * Buffer-》Input_Butter［1］
+Parameter-》b［2］ * Buffer-》Input_Butter［0］
-Parameter-》a［1］ * Buffer-》Output_Butter［1］
-Parameter-》a［2］ * Buffer-》Output_Butter［0］;
}
else
{
Buffer-》Output_Butter［2］=Buffer-》Input_Butter［2］;
LPB_Cnt++;
}
/* x（n） 序列保存 */
Buffer-》Input_Butter［0］=Buffer-》Input_Butter［1］;
Buffer-》Input_Butter［1］=Buffer-》Input_Butter［2］;
/* y（n） 序列保存 */
Buffer-》Output_Butter［0］=Buffer-》Output_Butter［1］;
Buffer-》Output_Butter［1］=Buffer-》Output_Butter［2］;
return （Buffer-》Output_Butter［2］）;
}
相关结构体定义如下。
typedef struct
{
//volatile
float Input_Butter［3］;
//volatile
float Output_Butter［3］;
}Butter_BufferData;
typedef struct
{
float a［3］;
float b［3］;
}Butter_Parameter;
自研飞控视频，链接如下：
四旋翼GPS户外定点篇（F330机架）
四旋翼GPS定点F450青山江边
此篇博客废话太多，疏于整理，难免有不正之处，欢迎交流指正！！！
下节讨论传感器延时修正的简单处理方法与惯性导航数据融合的细节部分。。。
举报
