作为无人机方面做嵌入式编写的飞控总结6--IMU惯性系统和GPS导航系统融合小结2（组合导航算法）

导：

3.基本理论：组合导航定义，想说的都在文献之中...重点关注红色下划线处

小结：

惯性系统和GPS导航系统各有优缺点，但在 误差传播性能上正好是互补的，前者长期稳定性差，但短期稳定性好，而后者正好相反;组合导航技术是指使用两种或两种以上的不同导航系统对同一信息源作测量，从这些测量值的比较值中提取出各系统的误差并校正之。

采用组合导航技术的系统称为组合导航系统。参与组合豹各导航系统称为子系统。

注意: 由于惯导系统具有自主性、隐蔽性、信息的全面性和宽频带等特有优点，所以 一般都以惯导系统作为组合导航系统的关键子系统;

由于惯导系统和GPS导航系统性能互补，所以，以该两子系统构造出组合导航系统是目前导航研究的热点。

4.分类    

    1．松散组合

松散组合是一种低水平的组合，GPS和惯导仍独立工作，用GPS辅助惯

导。分为以下两种：

(1)用GPS重调惯导

    这是一种最简单的组合方式，可以有两种工作方式：

一种是用GPS给出的位置、速度信息直接重调惯导系统的输出。实际上，就是在GPS工作期间，惯导显示的是6PS的位置和速度；GPs停止工作时。惯导在原显示的基础上变化，即GPS停止工作瞬fBJ时的位置和速度作为惯导系统的初值；

另一种是把惯导和GPS输出的位置和速度进行加权平均，其原理框图如图2．3所示，在短时间工作情况下，第二种工作方式精度较高。而长时『白】工作时，由于惯导误差随时间增长，因此惯导输出的加权随时问增长而减小，因而长时间工作时，性能和第一种工作方式基本相同。

(2)位置、速度组合(工程应用的关注重点)

位置、速度组合属于松散组合，采用卡尔曼滤波进行组合，如图3．4所示。用GP$和惯导输出的位置和速度信息的 差值作为量测值，经卡尔曼滤波，估计 惯导系统的误差，然后对惯导系统进行校正。

这种综合模式的优点是 组合工作比较简单，便于工程实现，而且两个系统仍能独立工作，使导航信息有一定余度。

缺点是GPS的位置和速度误差通常是时间相关的，而且卡尔曼滤波器要求量测噪声为 白噪声才能保证估计的无偏性。解决这个问题的方法有多种，常用的是（ 称它为“白”噪声，是因为它类似于光学中包括全部可见光频率在内的白光. 服从均匀分布； 高斯噪声 是指它的概率密度函数服从高斯分布（即正态分布）的一类噪声） ：

(i)加大综合滤波器的迭代周期

加大综合滤波器的迭代周期，使迭代周期超过误差的相关时间，在这个周期内可把量测误差作为白噪声处理。由于GPS的位置误差和速度误差相关时间长短不同，。因此可以把位置量测和速度量测分别处理，从而形成位置信息和速度信息交替使用的工作方式。这种工作方式比位置速度信息同时使用时精度有所降低，但计算工作量却大大减小，因而这种工作方式还是一种可取的工作方式。

(ii)把GP$滤波器和综合滤波器统一考虑，用分散滤波器进行设计;

把GP$滤波器和综合滤波器统一考虑，用分散滤波器进行设计。即把GPs滤波器作为子滤波器，把综合滤波器作为主滤波器，则综合系统的两个滤波器就构成了分散滤波器。用这个分散滤波器就不再要求GPS滤波器输出的位置和速度误差为白噪声，但是GPS滤波器和综合滤波器之间的通信量增大了。

2．紧密组合....(略)

5 方程分析

(1)组合导航的系统建立

首先需建立以惯导系统误差方程为基础的组合导航系统状态方程，并在组合导航系统误差方程的基础上建立组合导航系统的量测方程。采用卡尔曼滤波器为馁导系统误差提供最小方差估计，然后利用这些误差的估计值去修正惯导系统，以减少导航误差．同时，经过校正后的惯导系统又可以提供导航信息，以辅助GPS系统提高其性能和可靠性。

间接法，估计状态的误差，可以使滤波方程化为线性。其实，每个系统的真值在卡尔曼滤波器的观测量中已经被对消，此时系统方程和量测方程一般都是线性的，采用线性卡尔曼滤波器即可。

因此，只需建立GPS误差和惯导系统误差模型就可以了．而且间接法估计过程是与原系统无关的独立过程．对原系统来说，除了接受误差估计的校正外，也保持工作的独立性，这能发挥各个系统的特点。所以通常采用间接法方案。

误差方程为变系数微分方程，一般包括速度误差微分方程、经纬度误差微分方程以及三个平台误差角微分方程等。本文采取问接法实现卡尔曼滤波处理，取系统的误差作为状态。

(2)状态方程

1)INS的状态方程

位置误差方程

平台误差角方程

速度误差方程

2)．GPS的误差状态方程

定常速度情况下的系统模型。GPS的误差状态类似于INS系统，选位置误差和速度误差作为状态变。

     

 对于分布式组合，有两个卡尔曼滤波器分别处理INS和GPS的数据，两个滤波器的误差状态方程单独建立。对于全组合方式，只有一个卡尔曼滤波器，组合系统的误差状态方程除了有INS的15个状态变量，是否需要增加有关的误差状态变量，‘要根据被观测修正信息的选择来决定。

由于本文中选取位置、速度为观测信息，而GPS接收机的位置和速度误差一般是时间相关的，所以是有色噪声，建模困难，不能用状态扩充法加以处理，只能增加迭代周期。

综上所述组合导航系统中不增加GPS的状态变量。

 ===============枯燥理论的结束分割线====================================

6.代码和流程分析

基本思路：用GP$和惯导输出的位置和速度信息的差值作为量测值，经卡尔曼滤波，估计惯导系统的误差，然后对惯导系统进行校正。

1.扩展卡尔曼滤波 ~~~~