1. 组合导航的基本思想#
1.1 为什么要组合？#
导航的两类基本方法是定位和航位推算。不同导航系统具有互补性：
| 系统 | 主要优势 | 主要局限 |
|---|---|---|
| 惯性导航 INS | 自主性强；可连续、高更新率地输出位置、速度、姿态等参数 | 误差随时间积累 |
| GPS | 可提供位置、速度信息，用于约束惯导误差 | 相对INS更新率较低；遮挡等情况可导致信号中断 |
| 天文导航 | 可提供定位、定姿信息 | 数据更新率较低 |
组合导航：将两种或两种以上导航系统的信息融合，取长补短。 本章讨论的典型结构以INS为主体，其他系统提供辅助信息。
理想导航系统应兼顾：自主性、功能完备性、精度、环境适应性，以及体积、重量、成本等要求。
1.2 组合后解决什么问题？#
- 提高导航精度，抑制惯导误差及舒勒振荡的影响。
- 利用信息冗余进行故障检测、识别与工作模式切换，提高可靠性。
- 估计、校准惯性器件误差，降低对单个器件精度的要求。
- 支持动态初始对准和调整，缩短准备时间。
本章主线：利用两个系统的输出差值，估计误差，再校正导航结果或系统内部状态。
2. 卡尔曼滤波的模型与假设#
2.1 KF在做什么？#
卡尔曼滤波（Kalman Filter，KF）是一种基于状态空间模型的线性最小方差递推估计方法。
- 根据系统模型预测当前状态。
- 用当前观测修正预测结果。
- 同时递推估计误差的协方差，描述估计的不确定程度。
五个核心要素：状态向量及其协方差、系统模型、观测向量及其协方差、观测模型、滤波算法。
2.2 两个基本方程#
状态方程：
$$ X_k=\Phi_{k,k-1}X_{k-1}+\Gamma_{k-1}W_{k-1} $$观测方程：
$$ Z_k=H_kX_k+V_k $$| 符号 | 含义 | 维数 |
|---|---|---|
| $X_k$ | 待估计状态向量 | $n\times1$ |
| $Z_k$ | 观测向量 | $m\times1$ |
| $W_k$ | 过程噪声 | $r\times1$ |
| $V_k$ | 观测噪声 | $m\times1$ |
| $\Phi_{k,k-1}$ | 状态转移矩阵 | $n\times n$ |
| $\Gamma_{k-1}$ | 过程噪声输入矩阵 | $n\times r$ |
| $H_k$ | 观测矩阵 | $m\times n$ |
记忆：状态方程说明“状态怎样随时间变化”；观测方程说明“观测怎样反映状态”。
2.3 噪声与初值的假设#
以下递推公式采用最常见的零均值、过程噪声与观测噪声不相关情形：
$$ E[W_k]=0,\qquad E[V_k]=0 $$ $$ E[W_kW_j^T]=Q_k\delta_{kj},\qquad E[V_kV_j^T]=R_k\delta_{kj} $$ $$ E[W_kV_j^T]=0 $$其中，$\delta_{kj}=1$ （$k=j$ ），否则为0，表示噪声在不同时刻不相关。
初始状态的均值、协方差已知：
$$ E[X_0]=\mu_0,\qquad P_0=E[(X_0-\mu_0)(X_0-\mu_0)^T] $$且$X_0$ 与过程噪声、观测噪声不相关。
理解边界： 课件也提到非零均值和噪声互相关的情形；非零均值需要在模型中处理，存在互相关时不能直接套用本笔记的标准增益公式。高斯条件下，线性KF还可得到条件均值意义下的最优估计；一般情况下应记作线性最小方差估计。
3. 卡尔曼滤波递推过程（必会）#
记号：$\hat X_{k|k-1}$ 为利用截至$k-1$ 时刻的信息对$k$ 时刻的预测；$\hat X_{k|k}$ 为加入$k$ 时刻观测后的估计。上标$T$ 表示转置。
3.1 初始化#
$$ \hat X_{0|0}=\mu_0,\qquad P_{0|0}=P_0 $$3.2 时间更新：预测#
① 状态预测
$$ \boxed{\hat X_{k|k-1}=\Phi_{k,k-1}\hat X_{k-1|k-1}} $$② 预测误差协方差
$$ \boxed{ P_{k|k-1}=\Phi_{k,k-1}P_{k-1|k-1}\Phi_{k,k-1}^{T} +\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^{T} } $$含义：将已有估计及其不确定性传递到当前时刻，并加入过程噪声造成的不确定性。
3.3 量测更新：修正#
③ 新息：实际观测减去预测观测
$$ \boxed{\varepsilon_k=Z_k-H_k\hat X_{k|k-1}} $$④ 卡尔曼增益：确定修正权重
$$ \boxed{ K_k=P_{k|k-1}H_k^T \left(H_kP_{k|k-1}H_k^T+R_k\right)^{-1} } $$⑤ 状态修正
$$ \boxed{\hat X_{k|k}=\hat X_{k|k-1}+K_k\varepsilon_k} $$⑥ 更新估计误差协方差
$$ \boxed{P_{k|k}=(I-K_kH_k)P_{k|k-1}} $$递推顺序：预测状态 → 预测协方差 → 算新息 → 算增益 → 修正状态 → 更新协方差。
新息为零时，状态估计不发生修正；但观测仍可能降低估计的不确定性，因此协方差仍需更新。
4. 必辨：P、Q、R与工程实现#
4.1 三个矩阵分别描述什么？#
| 矩阵 | 描述对象 | 物理含义 |
|---|---|---|
| $P$ | 状态估计误差 | 对估计结果有多不确定；$n\times n$ |
| $Q$ | 过程噪声 | 状态演化模型受到多大的随机扰动；$r\times r$ |
| $R$ | 观测噪声 | 测量有多不准确；$m\times m$ |
- $P$ 的对角元素是各状态估计误差的方差，非对角元素是误差之间的协方差。
- $Q\succeq0$ ，$R\succ0$ ；有效的$P$ 应保持对称、半正定。
- $Q$ 描述模型扰动，$R$ 描述测量误差，$P$ 描述最终估计误差。
维数校正：课件第8页将$Q$ 写为$n\times n$ ；按第7页$W\in\mathbb R^r$ 的定义，应为$r\times r$ 。进入$P$ 预测方程的$\Gamma Q\Gamma^T$ 才是$n\times n$ 。
4.2 怎样理解增益的大小？#
用标量、$H=1$ 的情形理解：
$$ K=\frac{P^-}{P^-+R},\qquad P^-=P_{k|k-1} $$- 预测越不确定（$P^-$ 越大），越依赖观测修正。
- 观测噪声越大（$R$ 越大），越依赖模型预测。
- 增大$Q$ 通常会增大预测不确定性，使滤波器更积极地跟随观测。
这些是直观规律；多维系统中还受状态耦合和$H$ 的影响。
4.3 初值、调参与数值稳定性#
- 初值： 已知统计特性时，取初始均值和协方差。误差状态均值未知时，课件常取$\hat X_0=0$ ，并按可能的误差范围设置$P_0$ ；零初始估计不代表初始误差确定为零。
- 调参： 主要调整$P_0,Q,R$ 。课件建议先固定$P_0,Q$ 调整$R$ ，再视效果调整其他参数，兼顾收敛速度和稳定性。
- 计算量： 利用矩阵稀疏性、协方差对称性；状态传递和量测更新可采用不同频率。
- 舍入误差： 可能破坏$P$ 的对称性、半正定性，甚至产生负方差。
数值实现可使用课件给出的较稳定协方差更新形式（Joseph形式）：
$$ P_{k|k}=(I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T+K_kR_kK_k^T $$在所需逆矩阵存在时，也可写成信息形式：
$$ P_{k|k}^{-1}=P_{k|k-1}^{-1}+H_k^TR_k^{-1}H_k $$复习要求：熟记标准递推，理解其他形式用于改善数值计算；不必重复背诵所有等价公式。
5. KF怎样用于组合导航？#
对应课件：第19–26、51–54页。
5.1 直接法与间接法：区别在“估计什么”#
| 比较项 | 直接法 | 间接法 |
|---|---|---|
| 状态 | 导航参数本身，如位置、速度、姿态 | 导航参数误差及器件误差 |
| 主要输入 | 比力、角速度及其他系统提供的导航信息 | 不同系统同类导航参数的差值 |
| 输出 | 导航参数的估计值 | 误差估计值，用于校正 |
| 本章强调的特点 | 可能具有较强非线性；线性化和计算负担较大 | 小误差便于线性化；可保留各子系统独立解算及INS高更新率 |
课件以间接法为主要方案：先独立解算，再融合估计误差并校正。
5.2 为什么两系统的差值能用来估计误差？#
设真实导航参数为$X$ ，INS与辅助导航系统输出分别为：
$$ X_I=X+\Delta X_I,\qquad X_N=X+\Delta X_N $$则：
$$ \boxed{Z=X_I-X_N=\Delta X_I-\Delta X_N} $$作差消去真实值，留下两个系统的误差。 因而差值可以作为误差状态滤波器的观测，但不能直接当作INS的真实误差，仍需利用模型和噪声统计进行估计。
5.3 输出校正与反馈校正：区别在“校正哪里”#
输出校正（开环校正）：
$$ \boxed{\hat X=X_I-\widehat{\Delta X_I}} $$这里约定误差为“系统输出减真实值”，因此校正时相减。若定义校正后误差为$\hat X-X$ ，则：
$$ \hat X-X=\Delta X_I-\widehat{\Delta X_I} $$即最终导航误差由误差估计的准确程度决定；反向定义误差时，整体符号相反。
| 比较项 | 输出校正／开环 | 反馈校正／闭环 |
|---|---|---|
| 校正位置 | 系统对外输出 | 导航系统内部状态或误差源 |
| INS内部误差 | 仍可随时间积累 | 经反馈校正后保持较小 |
| 工程实现 | 较简单 | 较复杂 |
| 滤波器故障影响 | 较容易保持原INS独立运行 | 可能直接影响INS内部状态和输出 |
| 主要局限 | 长时间误差增大，破坏小误差线性化近似 | 对反馈实现及可靠性要求更高 |
| 课件建议场景 | INS精度较高、连续工作时间较短 | INS精度较低、连续工作时间较长 |
**理论与实际要分开：**在理想模型及最优滤波条件下，两种校正可以等效；工程中因线性化误差、模型失配和故障传播等因素，效果与可靠性存在区别。反馈有助于维持小误差条件，不代表所有模型误差都消失。
课件中闭环模型通过控制／校正项表示反馈作用：
$$ X_{k+1}=\Phi_{k+1,k}X_k+B_kU_k+W_k $$反馈量及其正负号应与误差定义一致。
5.4 集中式与分布式：区别在“怎样组织滤波器”#
- **集中式：**一个共同KF处理多个导航子系统的数据。
- **分布式：**各局部滤波器先处理各自观测，再将局部估计送入主滤波器融合。
三组分类不能混淆：直接／间接看估计对象；输出／反馈看校正位置；集中／分布看融合结构。
6. INS/GPS位置、速度组合示例#
对应课件：第31–50页。
6.1 掌握建模链条#
选误差状态 → 建立误差传播模型 → 构造INS与GPS的位置、速度差 → 离散化 → KF递推 → 校正INS。
GPS信息可用于约束INS误差、估计惯性传感器误差；INS也可辅助GPS跟踪。具体辅助能力取决于系统设计。
6.2 状态向量：本例为18维#
课件选取：
$$ X=\begin{bmatrix} \phi_E&\phi_N&\phi_U& \delta V_E&\delta V_N&\delta V_U& \delta L&\delta\lambda&\delta h& \varepsilon_{bx}&\varepsilon_{by}&\varepsilon_{bz}& \varepsilon_{rx}&\varepsilon_{ry}&\varepsilon_{rz}& \nabla_x&\nabla_y&\nabla_z \end{bmatrix}^{T} $$| 状态组 | 数量 | 含义 |
|---|---|---|
| $\phi_E,\phi_N,\phi_U$ | 3 | 平台误差角／姿态小误差 |
| $\delta V_E,\delta V_N,\delta V_U$ | 3 | 东、北、天速度误差 |
| $\delta L,\delta\lambda,\delta h$ | 3 | 纬度、经度、高度误差 |
| $\varepsilon_{bx},\varepsilon_{by},\varepsilon_{bz}$ | 3 | 陀螺随机常值漂移 |
| $\varepsilon_{rx},\varepsilon_{ry},\varepsilon_{rz}$ | 3 | 陀螺一阶马尔可夫漂移 |
| $\nabla_x,\nabla_y,\nabla_z$ | 3 | 加速度计误差 |
记忆：9个导航误差状态＋9个器件误差状态＝18维。 这里$L$ 为纬度，$\lambda$ 为经度；E/N/U分别表示东/北/天，x/y/z表示器件轴。
6.3 状态模型：记结构与误差传播关系#
$$ \dot X(t)=F(t)X(t)+G(t)W(t) $$ $$ F=\begin{bmatrix}F_N&F_S\\0&F_M\end{bmatrix} $$- $F_N$ ：导航误差之间的耦合。
- $F_S$ ：器件误差对导航误差的影响。
- $F_M$ ：器件误差自身的演化。
误差传播主线：陀螺漂移影响姿态；姿态误差与加速度计误差影响速度；速度误差进一步影响位置。 因此位置、速度观测还可能通过动态耦合帮助估计部分姿态和器件误差，具体能否估计取决于可观测性。
课件使用的器件误差模型：
$$ \varepsilon=\varepsilon_b+\varepsilon_r+\omega_g,\qquad \dot\varepsilon_b=0 $$ $$ \dot\varepsilon_r=-\frac{1}{T_g}\varepsilon_r+\omega_r, \qquad \dot\nabla_a=-\frac{1}{T_a}\nabla_a+\omega_a $$$T_g,T_a$ 为相关时间；$\omega$ 为相应驱动白噪声。捷联系统还需将器件轴误差变换至导航坐标系。
6.4 量测模型：6维位置、速度差#
位置量测按课件的“北、东、天”顺序写为：
$$ Z_p=\begin{bmatrix} (L_I-L_G)R_M\\ (\lambda_I-\lambda_G)R_N\cos L\\ h_I-h_G \end{bmatrix} =H_pX+V_p $$ $$ H_p=\begin{bmatrix} 0_{3\times6}&\operatorname{diag}(R_M,R_N\cos L,1)&0_{3\times9} \end{bmatrix} $$其中$R_M,R_N$ 分别为子午圈、卯酉圈曲率半径。乘曲率半径是为了把经纬度角差换成位置距离差；上式沿用课件第45–46页的几何近似。
为与前述状态中的E/N/U速度顺序一致，本笔记将速度量测统一按东、北、天排列：
$$ Z_v=\begin{bmatrix} V_{IE}-V_{GE}\\V_{IN}-V_{GN}\\V_{IU}-V_{GU} \end{bmatrix} =H_vX+V_v $$ $$ H_v=\begin{bmatrix}0_{3\times3}&I_3&0_{3\times12}\end{bmatrix} $$顺序校正：课件第48页的速度量测排列为N/E/U，而第38页状态排列为E/N/U。若量测保持N/E/U，$H_v$ 对应的速度块须交换前两行，不能直接写$I_3$ 。
合并得到：
$$ Z=\begin{bmatrix}Z_p\\Z_v\end{bmatrix} =\begin{bmatrix}H_p\\H_v\end{bmatrix}X +\begin{bmatrix}V_p\\V_v\end{bmatrix} =HX+V $$本例：$X$ 为18维，$Z$ 为6维，$H$ 为$6\times18$ 量测直接关联位置和速度误差，其他误差通过状态传播关系参与估计。
6.5 离散化#
在采样间隔$T$ 内把$F$ 近似视为常值时：
$$ \Phi_{k,k-1}=e^{FT} =I+FT+\frac{(FT)^2}{2!}+\cdots $$较小采样间隔下，可采用一阶近似$\Phi\approx I+FT$ ，并按精度要求增加展开项。离散后回到标准KF的状态、观测方程。
注意： 连续白噪声强度、离散驱动噪声协方差和传播到状态上的过程噪声协方差属于不同定义；不要脱离$\Gamma$ 及采样方式，直接把课件中的$Q(t)/T$ 当成协方差预测的加项。
GPS位置、速度误差可能具有时间相关性；课件第38页讨论通过增大量测更新间隔减弱相关性的影响。复习应抓住“实际量测未必满足理想白噪声假设”这一点。
7. 组合滤波器的设计流程#
- 系统分析： 分析误差与系统特性，选择与导航结果有关、可建模、可观测的状态，建立状态与观测模型。
- 数值仿真： 实现滤波算法，调整参数，检查精度、收敛与稳定性。
- 实验检验与完善： 在性能允许时减少状态，将被省略状态的影响适当计入过程噪声，降低计算负荷并进行实验验证。
四个注意点：
- 坐标系一致： 不同系统的信息必须变换至同一坐标系，量测分量顺序和单位也应一致。
- 可观测性： 不是状态越多越好；难于观测且次要的状态可考虑舍弃。可观测性与运动、量测配置有关。
- 初值合理： $P_0$ 反映可能的初始误差；若状态间存在相关性，非对角元素不应机械设零。
- 自适应能力： 运动和环境改变可能导致噪声统计变化，需据此调整滤波中的噪声参数。
8. 考前速记与自测#
8.1 一分钟速记#
| 要点 | 一句话答案 |
|---|---|
| 为什么组合？ | 利用INS连续、自主的特点与辅助定位信息互补，约束误差积累 |
| KF主线 | 根据模型预测，再根据新息修正 |
| 新息 | 实际观测减预测观测，不是已知的真实误差 |
| P/Q/R | 估计误差／过程噪声／观测噪声的协方差 |
| 间接法估计什么？ | 导航误差及器件误差 |
| 为什么作差？ | 消去共同的真实导航参数，形成误差观测 |
| 输出校正 | 修正对外输出，内部惯导误差仍可积累 |
| 反馈校正 | 校正内部状态，有利于保持小误差条件 |
| 本例维数 | 18维状态、6维位置速度量测 |
8.2 自测题#
- 写出KF状态方程、观测方程，并说明两者分别描述什么。
- 写出标准KF对过程噪声、观测噪声和初始状态的假设。
- 不看笔记写出状态预测、协方差预测、新息、增益、状态更新、协方差更新六式。
- $Q$ 与$P$ 有什么区别？为什么预测协方差中要有$\Gamma Q\Gamma^T$ ？
- 为什么$\hat X_0=0$ 不等于“初始误差为零”？
- 直接法与间接法的估计对象、输入、线性化特点分别是什么？
- 推导$X_I-X_N=\Delta X_I-\Delta X_N$ ，并解释为什么差值不是INS真实误差。
- 比较输出校正与反馈校正；说明两者理论等效结论的适用条件。
- 为什么长期运行时，输出校正可能破坏误差模型的线性化条件？
- 本例为什么是18维状态、6维量测？姿态和器件误差是否被直接量测？
复习顺序建议：先完成第2、3、4、6、8题，再回看INS/GPS的状态和量测建模。