卡尔曼滤波的学习

合集下载

卡尔曼滤波的基本原理

卡尔曼滤波的基本原理一、引言卡尔曼滤波是一种用于估计系统状态的算法，最初由卡尔曼于1960年提出。

它在航空航天、导航、机器人等领域得到了广泛应用。

本文将介绍卡尔曼滤波的基本原理。

二、状态方程和观测方程在介绍卡尔曼滤波之前，我们需要先了解两个重要的概念：状态方程和观测方程。

状态方程描述了系统的动态演化规律，通常采用微分方程或差分方程来表示。

观测方程描述了系统输出与状态之间的关系，通常采用线性或非线性函数关系来表示。

三、卡尔曼滤波的基本思想卡尔曼滤波的基本思想是通过对系统状态进行递推估计，不断修正预测值与实际值之间的误差，从而得到更加精确的状态估计结果。

具体来说，卡尔曼滤波将系统状态表示为一个高斯分布，在每个时刻根据观测数据和先验知识更新该高斯分布，并输出当前时刻的最优估计值。

四、离散时间下的卡尔曼滤波离散时间下的卡尔曼滤波是卡尔曼滤波的一种常见形式。

在这种情况下，状态方程和观测方程都采用离散时间模型表示。

假设系统的状态为x(k)，观测值为z(k)，则可以将状态方程和观测方程表示为：x(k+1) = F(k)x(k) + G(k)w(k)z(k) = H(k)x(k) + v(k)其中，F、G、H分别为状态转移矩阵、控制矩阵和观测矩阵，w、v 分别为过程噪声和测量噪声。

五、卡尔曼滤波的递推过程卡尔曼滤波的递推过程包括预测步骤和更新步骤两个部分。

预测步骤用于对系统状态进行预测，更新步骤用于根据观测数据修正预测值。

1. 预测步骤在预测步骤中，我们需要利用上一个时刻的估计值来预测当前时刻的状态。

2. 更新步骤在更新步骤中，我们需要利用观测数据来修正预测值。

卡尔曼滤波器原理之基本思想（一）

卡尔曼滤波器原理之基本思想（⼀）⼀、卡尔曼滤波器要解决的问题⾸先说⼀下卡尔曼滤波器要解决的是哪⼀类问题，这类系统应该如何建模。

这⾥说的是线性卡尔曼滤波器，顾名思意，那就是线性动态的离散系统。

这类系统可以⽤如下两个⽅程来表⽰：\[\begin{array}{l}x(n + 1) = {\bf{F}}(n + 1,n)x(n) + {v_1}(n) \\y(n) = {\bf{C}}(n)x(n) + {v_2}(n) \\\end{array}\] 其中： x(n)表⽰系统的状态 F（n+1,n)为状态转移矩阵，表⽰状态随时间的变化规律。

通俗的讲，从当前状态到下⼀个状态之间有什么关系。

C(n)表⽰观测值与状态的关系 y(n)表⽰状态的观测值 v1表⽰系统过程的噪声 v2表⽰观测过程中产⽣的噪声上⾯的两个⽅程中，第⼀个⽅程是过程⽅程，它表⽰系统状态x(n)随时间的更新过程。

第⼆个⽅程为测量⽅程，表⽰状态x(n)与测量结果y(n)的关系。

这⾥我们要先对这两个⽅程中的概念做下解释。

⾸先解释下状态这个概念。

状态是对系统特征进⾏的⼀个抽象，由预测系统未来特性时所需要的、与系统过去⾏为有关的最少数据组成。

这个概念不好理解吧！那么举个例⼦。

相信不少朋友在⽹上看到过有⼈拿来讲述卡尔曼滤波原理。

这⾥房间⾥真实的温度就是状态，它可以是⼀个参数，也可以是多个参数。

那么，⽤温度计测出来的值，就是这⾥的观测值y(n)。

再说⼀个例⼦，假如我们要对⼀个运动的物体进⾏跟踪，那么，物体的位移和速度完全可以表⽰这个运动物体所组成的系统的主要特征。

这时的状态就可以⽤⼀个具有位移和速度两个特征的向量来表⽰。

解释到这⾥，相信很多朋友已经正确理解了状态这个概念，它表⽰的是系统客观存在的真实特征。

再说⼀下系统状态与其观测值之间为什么有C(n)的存在，这⾥它表⽰的是观测值与状态的关系。

再拿室内测度测量来举例⼦，室内客观真实温度（未知量）做为这个系统中的状态，⽤温度计来测量这个状态。

卡尔曼滤波的基本原理

卡尔曼滤波的基本原理1. 任务名称卡尔曼滤波的基本原理2. 引言卡尔曼滤波是一种用于估计动态系统状态的方法，它通过融合系统测量和模型预测的信息，提供对系统状态的最优估计。

该滤波器在众多领域，如导航、信号处理、机器人技术等方面得到了广泛应用。

本文将详细介绍卡尔曼滤波的基本原理及其应用。

3. 卡尔曼滤波器的算法卡尔曼滤波器的算法主要由两个步骤组成：预测步骤和更新步骤。

在预测步骤中，根据系统的动力学模型，利用上一时刻的状态估计和模型进行预测；在更新步骤中，根据测量值和预测值之间的差异，对状态进行修正。

3.1 预测步骤预测步骤中，卡尔曼滤波器通过状态转移矩阵和控制向量对上一时刻的状态估计进行预测。

预测的状态向量可由以下公式表示：x k=Fx k−1+Bu k其中，x k表示当前时刻的状态估计，x k−1表示上一时刻的状态估计，F表示状态转移矩阵，B表示控制向量，u k表示当前时刻的控制输入。

预测的协方差矩阵可由以下公式表示：P k=FP k−1F T+Q其中，P k表示当前时刻的协方差矩阵，P k−1表示上一时刻的协方差矩阵，Q表示过程噪声的协方差矩阵。

3.2 更新步骤更新步骤中，卡尔曼滤波器将测量值与预测值进行比较，通过计算卡尔曼增益，对预测的状态进行修正。

卡尔曼增益的计算公式如下所示：K k=P k H T(HP k H T+R)−1其中，K k表示卡尔曼增益，H表示测量矩阵，R表示测量噪声的协方差矩阵。

修正后的状态向量可由以下公式表示：x k=x k+K k(y k−Hx k)修正后的协方差矩阵可由以下公式表示：P k=(I−K k H)P k3.3 初始化在使用卡尔曼滤波器之前，需要对状态向量和协方差矩阵进行初始化。

通常情况下，初始状态向量和协方差矩阵可通过经验估计或历史数据进行初始化。

4. 卡尔曼滤波器的应用卡尔曼滤波器具有很广泛的应用领域，下面将介绍其中几个典型的应用。

4.1 导航在导航领域，卡尔曼滤波器常用于姿态估计、位置估计和速度估计等方面。

卡尔曼滤波的原理与应用pdf

卡尔曼滤波的原理与应用一、什么是卡尔曼滤波卡尔曼滤波是一种用于估计系统状态的算法，其基本原理是将过去的观测结果与当前的测量值相结合，通过加权求和的方式进行状态估计，从而提高对系统状态的准确性和稳定性。

二、卡尔曼滤波的原理卡尔曼滤波的原理可以简单概括为以下几个步骤：1.初始化：初始状态估计值和协方差矩阵。

2.预测：使用系统模型进行状态的预测，同时更新预测的状态协方差矩阵。

3.更新：根据测量值，计算卡尔曼增益，更新状态估计值和协方差矩阵。

三、卡尔曼滤波的应用卡尔曼滤波在很多领域都有广泛的应用，下面列举了几个常见的应用场景：•导航系统：卡尔曼滤波可以用于航空器、汽车等导航系统中，实时估计和优化位置和速度等状态参数，提高导航的准确性。

•目标追踪：如在无人机、机器人等应用中，利用卡尔曼滤波可以对目标进行状态估计和跟踪，提高目标追踪的鲁棒性和准确性。

•信号处理：在雷达信号处理、语音识别等领域，可以利用卡尔曼滤波对信号进行滤波和估计，去除噪声和提取有效信息。

•金融预测：卡尔曼滤波可以应用于金融市场上的时间序列数据分析和预测，用于股价预测、交易策略优化等方面。

四、卡尔曼滤波的优点•适用于线性和高斯性：卡尔曼滤波适用于满足线性和高斯假设的系统，对于线性和高斯噪声的系统，卡尔曼滤波表现出色。

•递归性：卡尔曼滤波具有递归性质，即当前状态的估计值只依赖于上一时刻的状态估计值和当前的测量值，不需要保存全部历史数据，节省存储空间和计算时间。

•最优性：卡尔曼滤波可以依据系统模型和观测误差的统计特性，以最小均方差为目标，进行最优状态估计。

五、卡尔曼滤波的局限性•对线性和高斯假设敏感：对于非线性和非高斯的系统，卡尔曼滤波的性能会受到限制，可能会产生不理想的估计结果。

•模型误差敏感：卡尔曼滤波依赖于精确的系统模型和观测误差统计特性，如果模型不准确或者观测误差偏差较大，会导致估计结果的不准确性。

•计算要求较高：卡尔曼滤波中需要对矩阵进行运算，计算量较大，对于实时性要求较高的应用可能不适合。

卡尔曼滤波器的五个公式

卡尔曼滤波器的五个公式
卡尔曼滤波器（Kalman Filter）的五个公式如下：
1. 预测状态：
x̂_k = F_k * x̂_k-1 + B_k * u_k
其中，x̂_k为当前时刻k的状态估计值，F_k为状态转移矩阵，x̂_k-1为上一时刻k-1的状态估计值，B_k为外部输入矩阵，u_k为外部输入。

2. 预测误差协方差：
P_k = F_k * P_k-1 * F_k^T + Q_k
其中，P_k为当前时刻k的状态估计误差协方差矩阵，P_k-1为上一时刻k-1的状态估计误差协方差矩阵，Q_k为系统过程噪声的协方差矩阵。

3. 计算卡尔曼增益：
K_k = P_k * H_k^T * (H_k * P_k * H_k^T + R_k)^-1
其中，K_k为当前时刻k的卡尔曼增益矩阵，H_k为观测矩阵，R_k为观测噪声的协方差矩阵。

4. 更新状态估计值：
x̂_k = x̂_k + K_k * (z_k - H_k * x̂_k)
其中，z_k为当前时刻k的观测值。

5. 更新状态估计误差协方差：
P_k = (I - K_k * H_k) * P_k
其中，I为单位矩阵。

初学者的卡尔曼滤波——扩展卡尔曼滤波（一）

初学者的卡尔曼滤波——扩展卡尔曼滤波（⼀）简介已经历经了半个世纪的卡尔曼滤波⾄今仍然是研究的热点，相关的⽂章不断被发表。

其中许多⽂章是关于卡尔曼滤波器的新应⽤，但也不乏改善和扩展滤波器算法的研究。

⽽对算法的研究多着重于将卡尔曼滤波应⽤于⾮线性系统。

为什么学界要这么热衷于将卡尔曼滤波器⽤于⾮线性系统呢？因为卡尔曼滤波器从⼀开始就是为线性系统设计的算法，不能⽤于⾮线性系统中。

但是事实上多数系统都是⾮线性的，所以如果卡尔曼滤波器不能⽤在⾮线性系统中的话，那么它的应⽤范围就⾮常有限了。

如果真的是这样，卡尔曼滤波器可能早就寿终正寝或者过很久很久才会被⼈注意到。

幸运的是早期的学者们对这个问题理解的⾮常深刻，⽽且也找到了解决⽅法，就是扩展卡尔曼滤波（EKF）。

事实上世界上的第⼀个卡尔曼滤波也是扩展卡尔曼滤波，⽽不是线性卡尔曼滤波器。

扩展卡尔曼滤波有很久远的历史，如果说有⼀个⾮线性系统需要⽤到卡尔曼滤波的话，不必怀疑，先试试扩展卡尔曼滤波准没错。

因为他有很久远的历史，所以可以轻松的找到许多这⽅⾯的资料。

不过扩展卡尔曼滤波也不是⽆懈可击的，它有⼀个很严重的短板——发散。

使⽤扩展卡尔曼滤波的时候请务必记在⼼上，时刻提醒⾃⼰，这样设计滤波器其结果会发散吗？毫不夸张地说相对于线性卡尔曼滤波设计扩展卡尔曼滤波器的就是在解决发散问题。

发散问题解决了剩下的都是⼩事。

⼩结：扩展卡尔曼滤波器主要⽤于⾮线性系统；扩展卡尔曼滤波器会发散。

线性化的卡尔曼滤波器在讨论扩展卡尔曼滤波之前，⾸先要了解⼀下线性化卡尔曼滤波。

它和线性卡尔曼滤波器在滤波器的算法⽅⾯有同样的算法结构，⼀样⼀样的。

不⼀样的地⽅在于这两者的系统模型不同。

线性卡尔曼滤波器的系统本⾝就是线性系统，⽽线性化卡尔曼滤波器的系统本⾝是⾮线性系统，但是机智的⼤神们将⾮线性的系统进⾏了线性化，于是卡尔曼滤波就可以⽤在⾮线性系统中了。

对于⼀个卡尔曼滤波器的设计者，就不要去管你的模型到底是⼀开始就是线性系统还是⾮线性系统线性化得到的线性系统，反正只要是线性系统就好了。

卡尔曼滤波的原理说明(通俗易懂)

卡尔曼滤波的原理说明（通俗易懂）以下是为大家整理的卡尔曼滤波的原理说明（通俗易懂）的相关范文，本文关键词为尔曼,滤波,原理,说明,通俗易懂,尔曼,滤波,原理,说明,学，您可以从右上方搜索框检索更多相关文章，如果您觉得有用，请继续关注我们并推荐给您的好友，您可以在综合文库中查看更多范文。

卡尔曼滤波的原理说明在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。

跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名RudolfemilKalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。

1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。

1957年于哥伦比亚大学获得博士学位。

我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《AnewApproachtoLinearFilteringandpredictionproblems》（线性滤波与预测问题的新方法）。

如果对这编论文有兴趣，可以到这里的地址下载：/~welch/kalman/media/pdf/Kalman1960.pdf 简单来说，卡尔曼滤波器是一个“optimalrecursivedataprocessingalgorithm（最优化自回归数据处理算法）”。

对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。

他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。

近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2．卡尔曼滤波器的介绍（IntroductiontotheKalmanFilter）为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。

但是，他的5条公式是其核心内容。

结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

卡尔曼滤波详解一维卡尔曼滤波实例解析(五个公式以及各个参数的意义)

卡尔曼滤波详解一维卡尔曼滤波实例解析（五个公式以及各个参数的意义）一、问题描述假设我们有一个一维系统，我们想要估计这个系统的状态x。

我们可以通过一维传感器获得关于这个系统的观测z，但是这个观测会存在误差。

二、基本原理三、基本公式1.状态预测：我们首先假设系统可以通过一个线性方程来描述：x(k)=Ax(k-1)+B(u(k))+w(k)，其中x(k)代表系统在时刻k的真实状态，A是系统的状态转移矩阵，B是外部输入的影响矩阵，u(k)是外部输入，w(k)是系统状态预测过程中的噪声。

2.状态协方差预测：卡尔曼滤波同时也需要估计状态的不确定性，即状态协方差。

协方差可以通过以下公式进行预测：P(k)=AP(k-1)A^T+Q(k-1)，其中P(k)代表状态协方差矩阵，Q(k-1)是协方差预测过程中的噪声。

3.观测预测：将状态的估计值带入观测模型中，可以预测观测值：z^(k)=Hx^(k)，其中z^(k)代表预测的观测值，x^(k)代表状态的估计值，H是观测模型矩阵。

4.观测残差：观测残差即观测值与预测观测值之间的差异：y(k)=z(k)-z^(k)，其中y(k)代表观测残差。

5.状态更新：基于观测残差，我们可以通过以下公式更新状态的估计值：x(k)=x^(k)+K(k)y(k)，其中K(k)代表卡尔曼增益。

卡尔曼增益可以通过以下公式计算：K(k)=P(k)H^T(HP(k)H^T+R)^-1，其中R为观测噪声的方差。

四、参数含义1.状态转移矩阵A：描述系统状态k与状态k-1之间的转移关系。

2.外部输入矩阵B：外部输入对系统状态的影响矩阵。

3.外部输入u(k)：外部输入，可以是控制信号或者测量噪声。

4.状态预测噪声w(k)：在状态预测过程中引入的噪声。

5.状态协方差矩阵P：表示状态估计的不确定性，协方差矩阵的对角线上的元素越大，状态的不确定性越大。

6.状态协方差预测噪声Q(k)：在状态协方差预测过程中引入的噪声。

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

1.2 Kalman 滤波理论的基础在估计问题中，长考虑如下随机线性离散系统模型,11,11k k k k k k k X X W ----=Φ+Γ k k k k Z H X V =+k X 是系统的n 维状态向量，k Z 是系统的m 维观察向量。

根据状态向量和观察向量在时间上存在的不同对应关系，我们可以把估计问题分为滤波、预测和平滑，以上式所描述的随机线性离散系统为例，设,ˆk jX 表示根据j 时刻和j 以前时刻的观察值，对k 时刻状态k X 做出的某种估计，则按照k 和j 的不同对应关系，叙述如下：（1）当k=j 时，对,ˆk jX 的估计称为滤波，即依据过去直至现在的观察测量来估计现在的状态。

相应地，称,ˆk jX 为k X 的最有滤波估计值，简记为ˆk X 。

这类估计主要用于随机系统的实时控制。

（2）当k>j 时对,ˆk jX 的估计称为预测或外推，即依据过去直至现在的观察测量来预测未来的状态，并把,ˆk jX 称为k X 的最优预测估计值。

这类估计广泛应用于对系统未来状态的预测和实时控制。

（3）当k<j 时对,ˆk jX 的估计称为平滑或内插，即依据过去直至现在的观察测量去估计过去的历史状态，并称,ˆk jX 为k X 的最优平滑估计值。

这类估计广泛应用于通过分析实验或试验数据，对系统进行评估。

在预测、滤波和平滑三类状态估计问题中预测是滤波的基础，滤波是平滑的基础。

最早的估计方法是高斯提出的最小二乘法，最小二乘法没有考虑到被估参数和观测数据的统计特性，因此这种方法不是最优估计方法。

Wiener 滤波器采用频域设计法，运算复杂，解析求解困难，整批数据处理要求存储空间大，造成其适用范围及其有限，仅适用于一维平稳随机过程信号滤波。

Kalman 滤波采用了和Wiener 滤波相同的估计准则，二者的基本原理一致，但是kalman 滤波是一种时域滤波方法，采用状态空间方法描述系统，算法采用递推形式，数据存储量小，不仅可以处理平稳随机过程，也可以处理多维和非平稳随机过程。

关于系统过程噪声和观测噪声的统计特性如下：[][]0,0,0T k k j k kjTk k j k kj Tk j E W E W W Q E V E V V R E W V δδ⎧⎡⎤==⎣⎦⎪⎪⎡⎤==⎨⎣⎦⎪⎡⎤=⎪⎣⎦⎩如果被估计状态k X 和对k X 的观测量k Z 满足上式约束，系统过程噪声k W 和观测噪声k V 满足上式的假设，系统过程噪声方差阵k Q 非负定，系统观测噪声方差阵k R 正定，k 时刻的观测为k Z ，则k X 的估计ˆkX 可按下述方程求解：状态一步预测：,1,11ˆk k k k k X X ---=Φ 状态估计1,1ˆˆˆk k k k k k k X X K Z H X --⎡⎤=+-⎣⎦滤波增益矩阵1,1,1T Tk k k k k k k k k K P H H P H R ---⎡⎤=+⎣⎦一步预测误差方差阵,1,11,1,11,1T Tk k k k k k k k k k k k P P Q -------=ΦΦ+ΓΓ估计误差方差阵[][],1TT k k k k k k k k k k P I K H P I K H K R K -=--+其中1T k k k k K P H R -=其中[],1k k k k k P I K H P -=-111,1T k k k k k k P P H R H ----=+Kalman 滤波算法的特点：（1）由于Kalman 滤波算法将被估计的信号看作在白噪声作用下一个随机线性系统的输出，并且其输入输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳序列的滤波，而且特别适用于非平稳马尔科夫序列或高斯-马尔科夫序列的滤波，因此其应用范围是十分广泛的。

（2）由于Kalman 滤波的基本方程时间域内的递推形式，其计算过程是一个不断地“预测-修正”过程，在求解是不要求存储大量的数据，并且一旦观测到了新的数据，随时可以算的新的滤波值，因此这种滤波方法非常便于实时处理，计算机实现。

（3）由于滤波器的增益矩阵于观测无关，因此它可离线算出，从而可以减少实时在线计算量；在求滤波器增益矩阵k K 时要求一个矩阵的逆，既要计算1,1Tk k k k k H P H R --⎡⎤+⎣⎦，它的阶数之取决于观测方程的维数m 而m 通常是最小的这样，上面的求逆运算是比较方便的；另外在求解滤波器增益的过程中随时可以算得滤波器的精度指标k P ，其对角线上的元就是滤波误差向量各分量的方差。

（4）增益矩阵k K 与初始方差0P ，系统噪声方差阵1k Q -以及观测噪声方差阵k R 之间具有如下关系：由基本滤波方程可见，当k R 增大时，k K 就变小，噪声变大滤波增益就应取小。

如果0P 变小，1k Q -变小，因为0P 小表示初始估计较好，1k Q -变小表示系统噪声变小，于是增益矩阵应变小以便较小的修正。

扩展kalman 滤波在车辆GPS/dr 组合定位系统中的应用GPS/DR 组合系统状态方程的建立取组合定位系统的状态变量为[,,,,,]T e e e n n n X x v a x v a =,其中e x ，n x 分别为车辆东向和北向的位置分量；e v ，n v 分别为车辆东向和北向的速度分量；e a ，n a 分别为车辆东向和北向的加速度分量。

则得到组合定位系统连续的状态方程为：()()()Xt AX t U W t =++ 式中，1000000100010000000001000000110000en aa A ττ⎡⎤⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-⎢⎥⎣⎦ 001001e n e a n a a U a ττ⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦，00()00e n a a W t ωω⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦ea ωna ω分别为2(0,)ea σ2(0,)n a σ的高斯白噪声；ea τna τ分别为车辆东向和北向机动加速度变化率的相关时间常数；e a n a 分别为车辆东向和北向机动加速度分量的“当前”均值。

设采样周期为T ，将系统连续的状态方程离散化，得到系统离散的状态方程为,1,1k k k k k k k X X U W --=Φ++ 式4.79式中，()()()()()()[]T k e k e k e k n k n k n k X x v a x v a =,1(,1)(,1)[,]k k e k k n k k diag ---Φ=ΦΦ令1ee aa τ=，1nn aa τ=，则(,1)e k k -Φ，(,1)n k k -Φ为21(,1)1(1)01(1)00e e e a T e e a T e k k e a T T T e e e ααα------⎡⎤-++⎢⎥Φ=-⎢⎥⎢⎥⎣⎦21(,1)1(1)01(1)00n n n a T n n a T n k k na T T T e e e ααα------⎡⎤-++⎢⎥Φ=-⎢⎥⎢⎥⎣⎦[]123456Tk U u u u u u u =其中，21110.5(1)e Te e e e u T T e a αααα---⎡⎤=-++-⎣⎦21110.5(1)e T e e e e u T T e a αααα---⎡⎤=-++-⎣⎦ 12(1)e T e e u T e a αα--⎡⎤=--⎣⎦3(1)e T e u e a α-=-21140.5(1)n T n nn n u T T e a αααα---⎡⎤=-++-⎣⎦ 15(1)n T n n u T e a αα--⎡⎤=--⎣⎦6(1)n T n u e a α-=-式4.79就是所建立的GPS/DR 组合定位系统的状态方程。

GPS/DR 组合系统观测方程的建立将GPS 输出的东向位置信息obs e 北向位置信息obs n ，角速率陀螺的输出ω以及里程计（或车速计）在一个采样周期内输出的距离s 作为观测量，；里程计的刻度系数取为1.观测量和状态变量之间的关系如下1obs e e x v =+ 2obs n n x v =+122tan ()e n e e n n e nv v a v a t v v v ωωωεε-⎡⎤-∂=+=+⎢⎥∂+⎣⎦s s ε=于是系统连续的观测方程为：1222eobsnobsn e e ne nsxvexvnv a v aZv vsωεωε⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-==+⎢⎥⎢⎥⎢⎥+⎢⎥⎢⎥⎢⎣⎦⎣⎦⎢⎣1v2v分别为GPS接收机输出的东向位置和北向位置的观测噪声，可近似为21(0,)σ21(0,)σ的高斯白噪声；ωε为陀螺的漂移，近似为2(0,)ωσ的高斯白噪声；sε为里程计的观测噪声，近似为2(0,)sσ的高斯白噪声。

将观测方程离散化，得到系统离散的观测方程为[]k k kZ h X V=+（4.86）式中，()()Tk obs k obs k k kZ e n sω⎡⎤=⎣⎦[]()()()()()()22()()e kn kn k e k e k n kke k n kxxv a v ah Xv v⎡⎤⎢⎥⎢⎥⎢⎥-=⎢⎥+⎢⎥⎢⎥⎢⎥⎣⎦，1()2()()()kkkks kvvVωεε⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎣⎦从式（4.86）知，观测方程是非线性的。

采用扩展Kalman滤波进行线性化，将[]kh X在预测值,1ˆk kX-处按泰勒级数展开并忽略二次以上的高次项，得,1,1ˆˆk k k k k k k kZ h X H X X V--⎡⎤⎡⎤=+-+⎣⎦⎣⎦（4.87）化简得,1,1ˆˆk k k k k k k k kZ H X V h X H X--⎡⎤=++-⎣⎦（4.88）其中[],1ˆ123456100000000100000000k k kkk X Xkh XHh h h hXh h-=⎡⎤⎢⎥∂⎢⎥==⎢⎥∂⎢⎥⎣⎦2(,1)(,1)(,1)(,1)(,1)(,1)(,1)1222(,1)(,1)ˆˆˆˆˆˆˆ2ˆˆn k k e k k e k k n k k e k k n k k n k kn k k e k ka v v v a a vhv v-----------=⎡⎤+⎣⎦(,1)222(,1)(,1)ˆˆˆn k k n k k e k k vh vv ---=+ 2(,1)(,1)(,1)(,1)(,1)(,1)(,1)3222(,1)(,1)ˆˆˆˆˆˆˆ2ˆˆe k k e k k e k k n k k n k k e k k n k k n k k e k k a v v v a a v h vv---------+-=⎡⎤+⎣⎦(,1)422(,1)(,1)ˆˆˆe k k n k k e k k vh vv ----=+5ˆTv h =6ˆTv h =式（4.88）就是所建立的GPS/DR 系统线性离散的观测方程。