INAV-----integrated navigation组合导航。
对于多旋翼的位置姿态估计系统：
PX4原生固件如今已经默认使用EKF2了，另一种情况是 使用local_position_estimator, attitude_estimator_q。
对于INAV则已经弃用。所以如果是基础比较好的同仁，可以直接研究EKF2。
不过博主是新进小白，刚开始研究位置和姿态解算。从互补滤波入手或许是一个快速入门的选择。
INAV干了什么：
INAV，顾名思义，隶属于导航系统，且是组合导航系统。导航，简而言之，就是对运动物体运动状态的测量。
这个运动状态对于飞行器来讲，主要是三个线位置，三个线速率，三个欧拉角，三个角速度。
在PX4中，INAV提供了飞行器的三轴线位置和三轴线速率，是对飞行器线运动的测量。
INAV使用什么方法：
互补滤波。
既然是组合导航，自然是使用了多种传感器，需要多种传感器数据进行融合。互补滤波是一种融合两种或者多种
不同来源数据的方法。
INAV使用了什么传感器：
加速度计，GPS，PX4FLOW，气压计，vision，lidar，mocap。
加速度计，测量飞行器本体系下的三轴加速度；
GPS ，测量飞行器的经纬高，和北东地坐标系下的速度；
PX4FLOW， 测量飞行器在当前位置水平方向上的运动；
气压计，测量飞行器的当前位置的气压值，可转化为北东地坐标系下的海拔高度；
vision ，视觉定位，博主不具备，一般小型多旋翼也不具备；
lidar，测量飞行器当前位置的当地高度；
mocap，运动捕捉系统，一般不具备。
INAV 融合方法：
全部体现在下面两个函数。
inertial_filter_predict 估计函数。
void inertial_filter_predict(float dt, float x[2], float acc)
{
if (PX4_ISFINITE(dt)) {
if (!PX4_ISFINITE(acc)) {
acc = 0.0f;
}
x[0] += x[1] * dt + acc * dt * dt / 2.0f;
x[1] += acc * dt;
}
}
inertial_filter_correct 校正函数。
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) {
float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) {
x[1] += w * ewdt;
}
}
}
估计函数：这里虽然叫预估，但是实际就是数值积分。输入是上一时刻的状态X。X[0]是位置，X[1]是速率。
X[0] = X[0] + x[1] * dt + acc * dt * dt / 2.0f -------> s = v*t + 0.5*a*t*t 位移公式
X[1] = X[1] + acc*dt --------> v = a*t 速率公式
显然如果我们能够获得准确的acc，就能够通过上述公式经过积分获得飞行器的位置和速率。
那我们如何才能获得准确的acc呢？
acc是由加速度计测量得到的，但是acc掺杂了加速度计本身的测量噪声，机体的振动，载体的运动加速度等干扰因素。
如果直接使用，势必会影响积分的精度，而且积分本身就会产生累积误差。
显然直接使用加速度计测出的acc是不可行的。
这就需要用其他传感器对acc进行校正了。
校正函数：校正分为两个部分。
一个部分是对位置和速率的直接校正，这也是校正函数做的事情，可以称之为直接校正。
另一部分是对acc的校正，这是在程序过程中做的，后面会讲到，可以称之为间接校正。
校正函数的形参解释：
e 代表由加速度得到的位置和速率，与其他传感器得到的位置和速率分别做差得到的偏差。
w 代表权重，或者补偿系数。
所以校正函数的意思就是如果由加速度积分得到的位置和速率，与其他传感器得到的位置
和速率有偏差的时候，就用这个w乘以e再乘以dt补偿到加速度得到的位置和速率上。
这是为什么呢？
因为加速度积分得到的结果，随着时间的推移，数值会发散。而其他传感器的结果则是有
界的。所以，直接校正可以立即使加速度的积分结果趋近于真实结果，是对其发散的一种限制。
但是其他传感器数据的获取频率是比较低的，并不能满足实时性的要求。
所以加速度积分仍然是必须的。由此，便引出了间接校正。
我们认为加速度积分得到的数值的误差是由于acc不准确引起的，所以每一次直接校正后，都对
acc进行补偿，也就是间接校正。这样在其他传感器没有数据的时候，利用校正后的acc积分，
短时间内，精度也是可以接受的。
以上介绍了INAV模块的整体思路，整个程序就是估计和校正的循环往复的过程。
///
下面我们一起看看INAV的具体代码实现吧：
首先，介绍一下params.c中的参数。这些参数在程序中反复出现，从命名可以区分它们的含义和作用。
INAV+权重+方向+传感器+含义 默认值 最大 最小 简单描述
INAV_W_Z_BARO 0.5 10 0 气压计高度方向 位置补偿系数
INAV_W_Z_GPS_P 0.005 10 0 GPS高度方向 位置补偿系数，只用来校正气压计的零偏
INAV_W_Z_GPS_V 0 10 0 GPS高度方向 速率补偿系数
INAV_W_Z_VIS_P 5 10 0 视觉高度方向 位置补偿系数，只用来校正气压计的零偏
INAV_W_Z_LIDAR 3 10 0 雷达高度方向 位置补偿系数
INAV_W_XY_GPS_P 1 10 0 GPS水平方向 位置补偿系数
INAV_W_XY_GPS_V 2 10 0 GPS水平方向 速率补偿系数
INAV_W_XY_VIS_P 7 10 0 视觉水平方向 位置补偿系数
INAV_W_XY_VIS_V 0 10 0 视觉水平方向 速率补偿系数
INAV_W_MOC_P 10 10 0 运动捕捉系统 位置补偿系数
INAV_W_XY_FLOW 0.8 10 0 光流水平方向 位置补偿系数
***************************************************************************************************************************************
INAV_W_XY_RES_V 0.5 10 0 速度源丢失时，缓慢的降低估计的水平速度时，使用的权重
INAV_W_GPS_FLOW 0.1 1 0 光流有效时，用这个系数，降低GPS的位置和速率的补偿系数
INAV_W_ACC_BIAS 0.05 0.1 0 加速度计零偏估计 补偿系数
INAV_FLOW_K 1.35 10 0 光流线性度的系数
INAV_FLOW_Q_MIN 0.3 1 0 最小可以接受的光流质量
INAV_LIDAR_ERR 0.2 1 0 雷达最大误差，用于判断是否到达了新的表面
INAV_LAND_T 3 10 0 落地检测时间，没有高度改变，在低油门的时候，单位s
INAV_LAND_DISP 0.7 10 0 落地检测高度，进入该高度，触发落地检测
INAV_LAND_THR 0.2 1 0 落地检测油门，进入该油门，触发落地检测，最好为悬停油门的一半
INAV_DELAY_GPS 0.2 1 0 GPS延时，单位s
INAV_FLOW_DIST_X 0 1 -1 光流模块在X方向偏离质心的距离，单位m
INAV_FLOW_DIST_Y 0 1 -1 光流模块在Y方向偏离质心的距离，单位m
INAV_DISAB_MOCAP 0 1 0 0使能运动捕捉，1禁用运动捕捉，如果使用虚拟GPS，设为0
INAV_LIDAR_EST 0 布尔量，雷达用于高度估计
INAV_LIDAR_OFF 0 20 -20 雷达零偏，单位m
CBRK_NO_VISION 0 328754 0 禁用视觉输入，设为328754可以禁用视觉输入
这些参数主要是补偿用的系数，有一些判定条件的阈值，还有几个开关量。
下面开始介绍程序实现。
INAV的程序结构非常简单，并不是本文的重点，在这里就不进行介绍了。
主要介绍INAV估计和校正的实现函数：int position_estimator_inav_thread_main(int argc, char *argv[])。
所有的数据处理都在这个函数中。
先介绍下大致的工作流程：
主要分三大部分:
第一部分，是变量定义和初始化。还有就是对uorb的主题规定订购那些主题，发布那些主题。
第二部分，是计算气压高的零偏。我们知道气压计是能够测出海拔高度的，但是用海拔高度
表示，没有直观性。我们都利用当地高度来衡量飞机的高度。因此，在起飞之前
测出飞机起飞点的海拔就很有必要。这里是按多次取平均的方法计算原点高度的。
第三部分，是位置估计和校正。
下面着重介绍第三部分的程序:
通过上面的介绍，我们可以将位置估计和校正的过程分成三个环节。
1 计算合适的e
e就是估计值和测量值的差值。测量值是通过传感器直接测得的位置和速率的数值。通过消除这个e，使估计值始终不会发散。
2 计算合适的w
我们使用了很多种传感器。每个传感器能够提供的信息是有限的，并且精度也不一样。
所以，由每个传感器得出的e，需要乘以一个系数，表示它们在估计过程中的权重。
例如气压计只能提供高度位置信息，不能提供高度速率信息。gps既能提供xyz三个方向的位置信息，也能提供速率信息。
但是气压计高度方向的位置精度高，能够达到20~30cm。gps的高度方向的位置精度则只能达到10m。
这就决定了它们在融合过程中的权重会依据自身的特点而各有侧重。
另一方面，有些传感器会受到环境的影响，而导致其测量精度是时变的。其发送回来的数据包里面经常包含quality的数据。
这是用来衡量传感器的工作状态的值，根据这个值得大小，也许进行权重的调整。
3 合理的补偿acc
有了e和w就可以利用上面介绍的估计和校正函数进行数据融合了。但是这样实际上并没有发挥出加速度计本身的特点。
对于结果而言，只是把所有量测数据进行了加权。对于量测噪声也没有很好抑制作用。
而对于估计值，仍然使用acc的测量值进行积分的话，就失去了利用量测值进行补偿的作用。
这个地方说的比较模糊，后面会严格推倒公式，就一目了然了。
