INS/GPS组合导航

对比INS与GPS导航方法，二者都有其各自的优缺点。
惯性导航系统INS是一种全自主的导航系统，可以输出超过200Hz的高频信号，并且具有较高的短期测量精度。除了提供位置与速度之外还可以提供姿态信息。但由于算法内部存在积分，惯性传感器的误差会不断累积，使得长期导航误差无限制增长。
与INS相反，GPS具有良好的长期精度，导航误差大致为几米，设备成本低于100美元。但是，它短期精度与输出频率较低。一个常规的GPS接收机通常无法提供姿态信息，除非采用一些额外的硬件或软件。此外，全球卫星导航系统需要依靠至少3颗卫星（4颗）的信号，而卫星信号通常会受到高层建筑、树木、隧道、大气以及多路径效应的干扰。
从上述特点来看，INS与GPS具有较好的互补特性，将二者集成可以得到比单一导航系统稳定性更好、精度更高的导航方案。INS/GPS的组合导航系统可以输出高频率的导航参数信息（位置、速度、姿态），并且在长、短期的导航过程中均能具备较高精度。采用基于卡尔曼滤波的最优估计方法，对GPS和INS定位导航信息进行融合，可以得到可靠的导航解。GPS能够防止惯性数据漂移，INS能在GPS信号中断时提供位置、速度、姿态信息。

 姿态误差：

加速度误差：

 

 角速度误差：

4 松耦合的INS/GPS组合导航模型

4.1 系统模型

连续时间卡尔曼滤波的系统模型：

其中，G为噪声分布矢量，包含了与状态矢量相关的方差：

F为状态转换矩阵，包含了INS误差模型的各个部分【也就是之前的线性化模型】，可以总结为以下形式：

状态矢量为 位置、速度、姿态、加速度计、陀螺仪的误差分量：

 因此，INS/GPS 松耦合组合导航的系统模型 可以写成：

 将上式进行展开，可以较为清楚地看到系统状态是如何通过动态矩阵进行耦合的：

 同理，离散时间的系统模型为：

 

4.2 测量模型

离散的KF测量模型表示为：

 其中，等式右侧第二项代表零均值的测量噪声，协方差为Rk。
由于KF的状态矢量包含INS中的误差，因此对应的测量矢量由INS预测的速度、位置与GPS测量的位置、速度之差组成：

 注意：如果感觉GPS的速度不准确，也可以只把位置当作观测值！

Hk是tk时刻的测量矩阵，描述了在无噪声情况下通过状态变量的线性组合得到测量值Zk：

那么，完整的松耦合测量模型可写为:

 

将其展开得到：

 此外，还有两个重要的协方差矩阵Rk、Pk
Rk包含测量状态对角线上的方差，定义为：

 

 预测状态的协方差Pk同样是一个对角矩阵，由对角线上的方差组成：

 其中，每一个对角元素的方差项也是3x3的对角矩阵，分别与位置，速度，姿态，陀螺仪偏差和加速度计偏差有关。

4.3 INS/GPS松耦合总框图

5 紧耦合的INS/GPS组合导航模型

 此处讨论紧耦合的组合导航模型。
与第4部分类似，首先讨论从L系下的INS动态误差和测量模型；
接着给出GPS误差和测量误差模型；
最后通过kalman滤波实现综合系统模型与测量模型。

5.1 系统模型

INS part
连续时间kalman滤波 INS系统模型（与松耦合相同）：

FI：动态协方差矩阵
G：噪声分布矩阵
wI：白噪声

 GPS part
kalman滤波GPS系统模型：

状态矢量中包含：GPS接收机时钟误差和漂移，通过随机游走建模：

动态协方差矩阵为

 噪声分布矢量：

各分量分别为：时钟偏差的白噪声标准差，时钟漂移的白噪声标准差; 因此，完整的系统模型可以表示为：  

 结合INS与GPS的系统模型

将 INS part 与 GPS part 的方程代入，得到上式的展开式：

 

 离散时间的系统模型：

 

5.2 测量模型

kalman滤波的测量模型在离散时间上的表达式为：

对于紧耦合的系统来说，可用的观测数据为GPS的伪距以及伪距速率等测量值，因此，测量矢量即为INS估计值与GPS测量值之差：

对于M个卫星来说，方程又可写为：

 

 伪距测量值【具体内容在第三章】

GPS接收机获得第m个卫星的信号，可以通过以下模型来表示：

利用卫星导航信息可以计算出卫星的钟差和电离层误差，对流层误差也可以通过建立适当的模型进行计算。因此，在对GPS误差进行校正之后，可以将校正后的伪距写成：

等式右侧第三项为各种剩余误差。

第m个卫星到GPS接收机的实际物理距离为：

 

 则，伪距方程可重写为：

 

 其中：

 校正后的接收机位置定义为：

 其中：

 由于伪距测量方程是非线性的，因此在进行kalman滤波之前需要进行线性化（在xINS附近泰勒展开），对于函数 F(x,y,z)在线性化点（xi,yi.zi）的展开式为：

 将伪距方程在当前最优估计值（xINS, yINS, zINS）处线性化后，得到：

 由INS的输出定义的伪距测量为：

 因此，可以得到二者的差值：

 

 

 

 所以，伪距测量误差可归结为：

 对于M个可被观测的卫星，测量误差方程可表示为：

 

 对于ECEF大地坐标中的位置，需要使用以下关系将其转换成ECEF直角坐标：

 为了在kalman滤波中使用，上述方程组必须通过泰勒级数线性化：

 

 将上式代入到测量误差方程中，

 

得到最终的伪距测量模型：

 

伪距速率测量

 

 卫星和接收器运动产生的多普勒频移是二者相对速度在连线上的投影，与发射频率成正比，与光速成反比

 

 其中：

 根据多普勒测量，伪距速率可以通过下式进行计算：

而实际的伪距速率为：

 建立伪距测量的模型：

令接收机时间漂移： 

 则

得到的测量模型是关于deta（Vx）、deta（Vy）、deta（Vz）的函数，因此需要将上式转化为状态误差的形式。
由INS测量得到的伪距速率为：
在这里插入图片描述
其中，Vx,INS、Vy,INS、Vz,INS是INS在e坐标系下估计的接收机速率。
将（8.66）与（8.67）式做差，得到：

 

 (8.70)中

 将（8.70）写成状态空间的形式：

对于M个可被观测的卫星，伪距速率的测量误差方程可表示为：

 

 速度在L系与e系之间的关系可通过下式表示：

 代入R矩阵，得到：

 因此，伪距速率测量模型最终可写为：

 

5.3 总体测量模型

将5.2提供的伪距误差和伪距速率误差的测量模型结合起来，可以得到整体测量模型为：

 

紧耦合集成实现的框图如图所示：