当前位置： 首页 > news >正文

凡科可以建设多个网站吗中国建设教育协会网站培训中心

news 2025/12/11 22:36:36
凡科可以建设多个网站吗,中国建设教育协会网站培训中心,徐州鸿美网站建设,国展网站建设原文#xff1a;多维卡尔曼滤波 (kalmanfilter.net) 目录 前言 基本背景 状态外推方程 示例 - 飞机 - 无控制输入 示例 - 带控制输入的飞机 示例 – 坠落物体 状态外推方程维度 线性时不变系统 线性动态系统建模 状态外推方程的推导 状态空间表示形式 示例 - 等速…原文多维卡尔曼滤波 (kalmanfilter.net) 目录 前言 基本背景 状态外推方程 示例 - 飞机 - 无控制输入 示例 - 带控制输入的飞机 示例 – 坠落物体 状态外推方程维度 线性时不变系统 线性动态系统建模 状态外推方程的推导 状态空间表示形式 示例 - 等速运动体 高阶动态系统建模 示例 - 恒加速度运动体 示例质量弹簧阻尼系统 求解微分方程 无输入变量的动态系统 具有输入变量的动态系统 协方差外推方程 无过程噪声的估计不确定度 构造过程噪声矩阵 Q  离散噪声模型 连续噪声模型 选择哪种型号 测量公式 观察矩阵 H 缩放 状态选择 状态组合 测量方程尺寸 中期摘要 预测方程 状态外推方程 协方差外推方程 辅助方程 测量公式 协方差方程 状态更新公式 状态更新公式维度 协方差更新方程 协方差更新方程推导 卡尔曼增益 卡尔曼增益方程推导 简化协方差更新公式  总结 例子 示例 9 – 车辆位置估计 状态外推方程 协方差外推方程 测量公式 卡尔曼增益 状态更新公式 协方差更新方程 数值示例 示例分析 示例 10 – 火箭高度估计 状态外推方程 协方差外推方程 测量公式 卡尔曼增益 状态更新公式 协方差更新方程 数值示例 示例分析 前言 阅读“一维卡尔曼滤波”部分后您应该熟悉卡尔曼滤波的概念。在本节中我们推导出多维多变量卡尔曼滤波方程。 本教程部分介绍线性卡尔曼滤波器 LKF。LKF假设系统动力学是线性的。 到目前为止我们已经处理了一维过程例如估计液体温度。但许多动态过程具有两个、三个甚至更多的维度。 例如描述飞机在空间中位置的状态向量是三维的 描述飞机位置和速度的状态向量是六维的 ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​         描述飞机位置、速度和加速度的状态向量是九维的 ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​         假设一个恒定加速度动力学模型我们可以通过九个运动方程来描述时间\n\处的外推飞机状态 ​​​​​​​        ​​​​​​​        ​​​​​​​         通常的做法是用矩阵形式的单个方程描述多维过程。 首先写出所有这些方程是非常累人的;用矩阵表示法表示它们更短更优雅。 其次计算机在矩阵计算方面非常高效。以矩阵形式实现卡尔曼滤波可缩短计算运行时间。 以下章节以矩阵形式描述卡尔曼滤波方程。当然理论部分之后是完全求解的数值示例。 最后一章包括两个数值示例。在第一个例子中我们设计了一个没有控制输入的六维卡尔曼滤波器。在第二个示例中我们设计了一个带有控制输入的二维卡尔曼滤波器。 基本背景 状态外推方程 我想描述的第一个卡尔曼滤波方程是状态外推方程。         使用状态外推方程我们可以根据对当前状态的了解来预测下一个系统状态。它将状态向量从现在时间步长 n  外推到未来时间步长  n 1  。         状态外推方程描述了动态系统的模型。在文献中它也被称为 预测变量方程过渡方程预测方程动态模型状态空间模型         矩阵表示法中状态外推方程的一般形式为         其中 是时间步长 \ n 1 \ 处的预测系统状态向量是时间步长 \ n \ 处的估计系统状态向量是控制变量或输入变量 - 系统的可测量确定性输入是过程噪声或干扰 - 影响状态的不可测量输入是一个状态转换矩阵G是控制矩阵或输入转换矩阵将控件映射到状态变量            注意在文献中状态转移矩阵 \ F \ 有时用希腊字母 \ \Phi \ 表示。         下图提供了状态外推方程的示意图说明。         状态变量可能表示我们想知道的系统属性。         例如移动的车辆具有三个属性位置、速度和加速度。         您可能会问自己哪些属性是状态变量哪些属性是系统的输入 移动机械系统具有位置、速度、加速度和阻力等属性。作用在系统上的力应被视为外部强迫函数即控制状态矢量恒定加速度情况下的位置和速度的系统的输入。牛顿第二定律告诉我们 F ma 。因此我们可以将加速度视为系统的外部输入。位置和速度是感兴趣的主要状态变量。         例如在弹簧系统中施加到弹簧 Ft  的力是输入 ut而弹簧位移 xt 是系统状态。         对于下落的物体输入是重力 和阻力 而物体高度  ht 和速度vt 是系统状态。          注意过程噪声\w_{n} \通常不会直接出现在感兴趣的方程中。相反该术语用于模拟协方差外推方程中的不确定性。         让我们看一下状态外推方程的几个例子。 示例 - 飞机 - 无控制输入         在此示例中我们定义了飞机的状态外推方程假设一个恒定的加速度模型。         在此示例中没有控件输入。         我们将假设没有控制输入。我们将在下一个示例中看到控件输入。         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​         考虑一架飞机在三维空间中以恒定加速度移动。描述笛卡尔坐标系中估计的飞机位置、速度和加速度的状态向量 xyz 为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   注意不要混淆估计的状态向量 粗体字体和 x  轴处的估计飞机位置该轴由 表示法线字体。这两个变量在文献中用同一个字母表示我试图保持一致。         状态转换矩阵  F  为                    状态外推方程为          矩阵乘法结果         ​​​​​​​        ​​​​​​​           示例 - 带控制输入的飞机         此示例与前面的示例类似但现在我们有一个传感器连接到飞行员的控件因此我们根据飞行员的命令获得了有关飞机加速的其他信息。         描述笛卡尔坐标系中估计的飞机位置和速度的状态向量 为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   描述笛卡尔坐标系 xyz中对照向量的飞机加速度的控制向量为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   状态转换矩阵 F 为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   控制矩阵 G 为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   状态外推方程为   示例 – 坠落物体         考虑一个自由落体的物体。状态向量包括高度 h 和对象的速度         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                  状态转换矩阵  F  为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                  控制矩阵  G  为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                  输入变量为         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                  其中 g  是引力加速度。         我们没有测量加速度的传感器但我们知道对于下落物体加速度等于g 。         状态外推方程如下所示         ​​​​​​​        ​​​​​​​                  矩阵乘法产生以下结果         ​​​​​​​        ​​​​​​​        ​​​​​​​           状态外推方程维度 下表指定了状态外推方程变量的矩阵维度                变量描述\                  x状态向量\                   F状态转换矩阵                   u输入变量\                  G控制矩阵                   w过程噪声矢量 线性时不变系统         线性卡尔曼滤波器假设LTI线性时不变系统模型。         那么什么是“线性”什么是“时不变”         线性系统由方程组描述其中变量永远不会彼此相乘而只是与常数相加然后相加。线性系统用于描述变量之间的静态和动态关系。         线性系统是其输出函数 满足以下等式的系统         ​​​​​​​        ​​​​​​​                  其中          a  和 b  是常数实数          g  和  h  是自变量  t  的任何任意函数 线性系统遵循两个基本规则 您可以“分解”恒定乘法比例因子上面的a 和  b 。系统对输入总和的响应分别是对每个输入的响应总和。         时不变系统具有不是时间直接函数的系统函数。         让我们以增益为 G 10 的放大器为例。         这个系统是时不变的。尽管系统的输出随时间变化但系统功能与时间无关。         时不变系统是指输入序列中的时间延迟或移位导致系统输出序列中的等效时间延迟的系统。   线性动态系统建模         嗯这很容易描述飞机的动态模型。我想你从高中就熟悉牛顿的运动方程。         本章概括了任何线性动态系统的动态模型推导。以下描述包括积分方程和微分方程。         本章是本教程中最具挑战性的章节。理解卡尔曼滤波原理不需要它。如果您对这个数学感到不舒服 - 请随时跳过本章。         就我而言我试图使我的解释尽可能简单易懂当然我提供了现实生活中的例子。 状态外推方程的推导         我们的目标是以以下形式推导出状态外推方程           为此我们需要对动态系统进行建模。换句话说要弄清楚动态系统的状态空间表示。以下两个方程是 LTI 系统的状态空间表示 哪里                                    x是状态向量                                 y是输出向量                                 A是系统的动态矩阵                                 B是输入矩阵                                 C是输出矩阵                                 D是馈通矩阵           要找到状态转移矩阵   F 和输入转换矩阵 G 我们需要求解状态空间微分方程。         下图总结了状态外推方程推导的过程。 状态空间表示形式         您可能想知道为什么状态空间表示形式必须采用以下形式         ​​​​​​​        ​​​​​​​        ​​​​​​​                   许多求解微分方程的计算机软件包都需要这种表示。         描述状态空间表示的最佳方式是通过示例。 示例 - 等速运动体         由于没有施加到身体上的外力因此系统没有输入         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                 状态空间变量 xt是物体的位移  pt 和速度vt。         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                     注意为了避免状态向量 xt粗体字体和沿 x  轴的身体位置由  xt 表示正常面字体我用 pt 表示身体位置。         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   我们得到了微分形式的第一个方程。         动态系统输出 yt 是主体位移 pt它也是状态变量。         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   我们完成了。 高阶动态系统建模         许多动态系统模型由高阶微分方程描述。微分方程的阶数是微分方程中最高导数的数。         为了解决高阶方程我们应该通过定义新变量并将它们代入最高阶项来将其简化为一阶微分方程。 微分方程阶的简化算法         一般的  n -th 阶线性微分方程可以表示为  n  一阶微分方程组。         动态系统的高阶控制方程如下所示         ​​​​​​​                  控制方程完全表征了系统的动态状态。         减少方程阶数 隔离最高阶导数         ​​​​​​​            y 及其第一个 n - 1 导数是这个系统的状态。         2.定义一个新变量         设置 我们写         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                 现在函数  是状态变量。        3.取状态变量的导数         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                  4. 将隔离的 项参见步骤 1代入最后一个等式         5. 使用向量矩阵表示法表示生成的方程组           就是这样。我们得到了以下形式的状态空间方程         ​​​​​​​        ​​​​​​​        ​​​​​​​                 其中   让我们看两个例子。 示例 - 恒加速度运动体 在这个例子中有一个外力施加到身体上。 具有恒定加速度的运动物体的控制方程是牛顿第二定律         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​         其中 p 是身体位置位移 m 是体重 F  是施加在身体上的外力         我们将把降阶算法应用于控制方程。 隔离最高阶导数         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                  2.定义新变量  和                                                                                              和 是状态变量。          3. 取状态变量的导数                                                            4.在最后一个等式中插入隔离的 项请参阅步骤 1         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   5.使用向量矩阵表示法表示生成的方程组         ​​​​​​​        ​​​​​​​        ​​​​​​​                   请记住 。用 v 表示  会更有意义因为 是体速。          我们可以将上面的等式重写如下         ​​​​​​​        ​​​​​​​        ​​​​​​​                                           或者         ​​​​​​​        ​​​​​​​        ​​​​​​​                   其中 a  是由施加的力 F 产生的身体加速度。         我们有一个公式形式如下         ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​                   状态空间变量 xt是物体的位移  pt  和速度  vt 。         动态系统输出 yt 是主体位移  pt它也是状态变量的一个元素。         ​​​​​​​        ​​​​​​​        ​​​​​​​           示例质量弹簧阻尼系统         质量弹簧阻尼器系统包括三个基本要素 质量 - 惯性元件弹簧 - 弹性元件阻尼器 - 摩擦元件         每个元素都有以下两种可能的能量行为之一 存储提供给它的所有能量通过“摩擦”效应将所有能量耗散成热量         质量将能量存储为动能。         当弹簧从其原始长度压缩时它将能量存储为势能。         阻尼器以热量的形式耗散能量。         这三个组件质量、弹簧和阻尼器可以在一般意义上模拟任何动态响应情况。         该系统的力图如下所示。         弹簧力与质量的位置位移  p 成正比。         粘性阻尼力与质量的速度 成正比。         牛顿第二定律指出         ​​​​​​​        ​​​​​​​        ​​​​​​​                   我们继续求和力并应用牛顿第二定律         ​​​​​​​        ​​​​​​​        ​​​​​​​                   其中         \ p \ 是身体位置位移         \ m \ 是体重         \ F \ 是施加在身体上的外力         \ k \ 是弹簧常数         \ c \ 是阻尼系数         该方程是一个控制方程可以完全表征系统的动态状态。         我们将把降阶算法应用于控制方程。     更多示例 您可以在 ShareTechnote 网站上找到许多状态空间建模的精美示例。 求解微分方程 请记住对于我们的卡尔曼滤波模型我们需要确定以下形式的状态外推方程           为了达到这个目标我们将求解描述状态空间表示的微分方程。         我们可以使用计算机软件来解决微分方程或自己做。         让我们看看如何求解微分方程。 无输入变量的动态系统         没有外部输入的LTI动态系统可以用一阶微分方程来描述           其中  A 是一个系统动力学矩阵。         我们的目标是找到状态转换矩阵  F。         我们需要求解微分方程来找到  F 。         在单个维度中微分方程如下所示           整合双方可产生以下结果           求解积分           同样在多维情况下对于           解决方案是           我们找到了状态转换矩阵 F :            是一个矩阵指数。         矩阵指数可以通过泰勒级数展开计算           因此           其中 是一个单位矩阵。 示例续 - 等速运动体         现在我们可以找到状态转换矩阵F对于等速运动方程。         以下一组微分方程可以描述恒定速度动力学模型。   以矩阵形式  让我们计算         ​​​​​​​                   由于   的更高幂也等于 0。         现在我们可以找到恒定速度模型的状态转换矩阵   具有输入变量的动态系统         对于零阶保持采样假设输入是分段常数状态空间方程的一般解形式为           由以下给出           如果我们删除输入变量 我们得到上一章中得出的解决方案。         我不打算证明这一点——你可以在汤姆·M·阿波斯托尔的“微积分”教科书第二版、定理 8.3 或任何其他微积分教科书中找到证明。         现在让我们求解恒加速度运动体的状态空间方程和质量-弹簧-阻尼器系统示例。 示例续 - 恒加速度运动体         回想一下恒定加速度运动体的状态空间表示为           其中           让我们求解等式           结果 F           我们已经解决了             查找 G           的一般公式是幂级数            的较高幂也等于 0。           现在我们可以编写状态外推方程   示例续 - 质量弹簧阻尼系统         回想一下质量弹簧阻尼器系统的状态空间表示为           在这个例子中矩阵指数的计算并不容易因为 A 的高幂不为零。         此微分方程的解超出了本教程的范围。 协方差外推方程 我假设读者已经熟悉协方差外推预测的概念。我们已经在“一维卡尔曼滤波”部分中遇到了协方差外推方程或预测变量协方差方程。在本节中我们用矩阵符号推导出卡尔曼滤波协方差外推方程。 协方差外推方程的一般形式由下式给出 是当前状态的估计值协方差矩阵的不确定性 是下一个状态的预测协方差矩阵的不确定性 F 是我们在“线性动态系统建模”部分中推导出的状态转换矩阵 Q 是过程噪声矩阵 无过程噪声的估计不确定度 假设过程噪声等于零  Q0则 推导相对简单。我在“基本背景II”部分中展示了 其中向量  x  是系统状态向量。 因此 根据状态外推方程 因此 应用矩阵转置属性 构造过程噪声矩阵 Q  如您所知系统动态描述如下 其中 是时间步长  n   处的进程噪声。 我们已经在“一维卡尔曼滤波器”部分讨论了过程噪声及其对卡尔曼滤波器性能的影响。在一维卡尔曼滤波器中过程噪声方差用  q  表示。 在多维情况下过程噪声是一个协方差矩阵用 表示。 我们已经看到过程噪声方差对卡尔曼滤波器的性能有关键影响。太小的  q   会导致滞后错误see Example 7。如果  q  值太高卡尔曼滤波器将遵循测量值see Example 8并产生噪声估计值。 过程噪声可以在不同的状态变量之间独立。在这种情况下过程噪声协方差矩阵 是对角矩阵 过程噪声也可能取决于。例如恒定速度模型假定加速度为零 a0。但是加速度 的随机方差会导致速度和位置的方差。在这种情况下过程噪声与状态变量相关。 环境过程噪声有两种模型。 离散噪声模型连续噪声模型 离散噪声模型 离散噪声模型假设噪声在每个周期不同但在周期之间是恒定的。 对于等速度模型过程噪声协方差矩阵如下所示 我们用模型的随机加速度方差来表示位置和速度方差和协方差。 我们使用“基本背景II”部分中的期望算术规则推导出矩阵元素。 现在我们可以将结果代入 矩阵 有两种方法可以更快地构造 矩阵。 使用状态转换矩阵进行投影 如果动态模型不包含控制输入我们可以使用状态转换矩阵在动态模型上投影加速度 的随机方差。 让我们定义一个矩阵 过程噪声矩阵为 对于运动模型矩阵由下式给出 使用控制矩阵进行投影 如果动态模型包含控制输入我们可以更快地计算矩阵。我们可以使用状态转移矩阵在动态模型上投影加速度 的随机方差。 其中 是控制矩阵或输入转换矩阵。 对于运动模型矩阵由下式给出 您可以使用上述方法来构造离散 矩阵。 连续噪声模型 连续模型假设噪声随时间连续变化。 为了推导出连续模型 的过程噪声协方差矩阵我们需要随着时间的推移对离散过程噪声协方差矩阵进行积分。 选择哪种型号 在回答此问题之前您需要为过程噪声方差选择正确的值。您可以使用随机统计公式进行计算也可以根据您的工程实践选择一个合理的值最好。 在雷达世界中取决于目标特性和模型完整性。对于机动目标如飞机应该相对较高。对于非机动目标如火箭您可以使用较小的 。模型完整性也是选择过程噪声方差的一个因素。如果您的模型包含空气阻力等环境影响则过程噪声随机性的程度较小反之亦然。 选择合理的过程噪声方差值后应选择噪声模型。它应该是离散的还是连续的 这个问题没有明确的答案。我建议尝试这两种模型并检查哪一个在卡尔曼滤波器上表现更好。当 非常小时可以使用离散噪声模型。当较高时最好使用连续噪声模型。 测量公式 到目前为止我们已经处理了未来。我们推导出了两个卡尔曼滤波预测方程 状态外推方程协方差外推方程 从现在开始我们将处理现在。让我们从测量方程式开始。 在“一维卡尔曼滤波器”部分我们用   表示测量值。 测量值表示除测量设备引起的随机测量噪声  之外的真实系统状态。 每次测量的测量噪声方差  可以是恒定的 - 例如精度为 0.5kg标准偏差的秤。另一方面每次测量的测量噪声方差  可能不同 - 例如精度为 0.5%标准偏差的温度计。在后一种情况下噪声方差取决于测量的温度。 矩阵形式的广义测量方程由下式给出 是测量向量是真实的系统状态隐藏状态是一个随机噪声矢量H是一个观察矩阵 观察矩阵 H         在许多情况下测量值不是所需的系统状态。例如数字电温度计测量电流而系统状态是温度。需要将系统状态输入转换为测量输出。         观察矩阵  H  的目的是使用线性变换将系统状态转换为输出。以下章节包括观察矩阵用法的示例。 缩放         测距仪向目的地发送信号并接收反射回波。测量是信号发送和接收之间的时间延迟。系统状态是范围。         在这种情况下我们需要执行缩放          其中         c 是光速          是范围        是测量的时间延迟 状态选择         有时某些状态是测量的而其他状态则不是。例如五维状态向量的第一、第三和第五个状态是可测量的而第二和第四个状态是不可测量的   状态组合         有时可以测量状态的某种组合而不是每个单独的状态。例如三角形边的长度可能是状态并且只能测量总周长   测量方程尺寸         下表指定了测量公式变量的矩阵维度    中期摘要 这是一个停下来做一个简短总结的好地方。在进一步讨论之前我想总结一下我们迄今所学到的情况。 您还记得“一维卡尔曼滤波部分”如果您不记得了请再次查看卡尔曼滤波计算基于五个方程。 两个预测方程 状态外推方程 - 根据已知的当前估计预测或估计未来状态。协方差外推方程 - 预测中不确定性的度量。 两个更新公式 状态更新方程 - 根据已知的过去估计和当前测量估计当前状态。协方差更新方程 - 估计中不确定性的度量。 卡尔曼增益方程 – 计算更新方程所必需的。卡尔曼增益是测量和过去估计的“加权”参数。它定义了过去估计的权重和估计当前状态的测量权重。 到目前为止我们已经学习了矩阵符号中的两个预测方程和计算主方程所需的几个辅助方程。 预测方程 状态外推方程 矩阵表示法中状态外推方程的一般形式为 是时间步长 \ n 1 \ 处的预测系统状态向量是时间步长 \ n \ 处的估计系统状态向量是控制变量或输入变量 - 系统的可测量确定性输入 是过程噪声或干扰 - 影响状态的不可测量输入F是状态转移矩阵G是控制矩阵或输入转换矩阵将控件映射到状态变量 协方差外推方程 协方差外推方程的一般形式由下式给出 是当前状态估计的不确定性协方差矩阵是下一个状态估计预测的不确定性协方差矩阵F是我们在“线性动态系统建模”部分中推导出的状态转换矩阵Q是过程噪声矩阵 辅助方程 测量公式 矩阵形式的广义测量方程由下式给出 协方差方程 对应于过程和测量噪声的术语 和 通常不会直接出现在计算中因为它们是未知的。 相反这些项用于模拟方程本身中的不确定性或噪声。 所有协方差方程都是协方差矩阵形式为 ​​​​​​​        ​​​​​​​         即对平方误差的期望。有关更多详细信息请参阅基本背景 II 部分。 测量不确定度 测量不确定度由下式给出 \是测量值的协方差矩阵\是测量误差 过程噪声不确定性 过程噪声不确定度由下式给出 \是过程噪声的协方差矩阵\是过程噪声 估计不确定性 估计不确定性由下式给出 \是估计误差的协方差矩阵\是估计误差\是真实的系统状态隐藏状态\是时间步长 \ n \ 处的估计系统状态向量 状态更新公式 此页面是本教程中最短的页面。我在“\ \alpha -\beta -\gamma \ 滤波器”部分和“一维卡尔曼滤波器”部分中提供了对状态更新方程的广泛描述。 矩阵形式的状态更新方程由下式给出 \是时间步长 \ n \ 处的估计系统状态向量\是时间步长 \ n - 1 \ 处的预测系统状态向量\是卡尔曼增益\是一种度量\H是一个观察矩阵 您应该熟悉状态更新方程的所有分量但矩阵表示法中的卡尔曼增益除外。我们将在以下章节中推导出卡尔曼增益。 您应该注意尺寸。例如如果状态向量有 5 个维度而只有 3 个维度是可测量的第一、第三和第五个状态 观察矩阵将是一个 矩阵 创新(   )产生 卡尔曼增益维度应为  5X 3 。 状态更新公式维度 下表指定了状态更新公式变量的矩阵维度 协方差更新方程 协方差更新方程由下式给出 \是当前状态估计的不确定性协方差矩阵\是当前状态的先验估计不确定性协方差矩阵在前一个状态预测是卡尔曼增益\H是观察矩阵\是测量不确定度测量噪声协方差矩阵\ I是一个单位矩阵( 方阵主对角线上有 1其他地方有零 协方差更新方程推导 本节包括协方差更新方程推导。你们中的一些人可能会觉得它太详细了但另一方面它会帮助其他人更好地理解。 如果您不关心派生可以跳到下一个主题。 对于推导我使用以下四个等式 我们推导出当前估计不确定性 作为卡尔曼增益 的函数。 卡尔曼增益 最后一个方程是卡尔曼增益方程。 矩阵符号中的卡尔曼增益由下式给出 \是卡尔曼增益\是当前状态的先验估计不确定性协方差矩阵在上一步预测\ H是观察矩阵\是测量不确定度测量噪声协方差矩阵 卡尔曼增益方程推导 本章包括卡尔曼增益方程的推导。如果您不关心派生可以跳到下一个主题。 首先让我们重新排列协方差更新方程 卡尔曼滤波是最佳滤波。因此我们寻求一个卡尔曼增益以最小化估计方差。 为了最小化估计方差我们需要最小化协方差矩阵 的主对角线从左上角到右下角。 方阵主对角线之和就是矩阵的迹线。因此我们需要最小化  tr。为了找到产生最小值所需的条件我们将 的跟踪与 进行区分并将结果设置为零。 简化协方差更新公式  在许多教科书中您可以找到协方差更新方程的简化形式 要推导协方差更新方程的简化形式请将卡尔曼增益方程代入协方差更新方程。 警告这个等式更优雅更容易记住并且在许多情况下表现良好。然而计算 卡尔曼增益时的微小误差由于舍入可能会导致巨大的计算误差。减法  由于浮点误差可能会导致非对称矩阵。计算卡尔曼增益时的微小误差由于舍入可能导致巨大的计算误差。减法 由于浮点误差可能导致非对称矩阵。 总结 我们已经用矩阵符号推导出了所有五个卡尔曼滤波方程。让我们把它们放在一个页面上。 卡尔曼滤波在“预测-校正”循环中运行如下图所示。 初始化后卡尔曼滤波器将在下一步预测系统状态。它还提供了预测的不确定性。 收到测量值后卡尔曼滤波会更新或校正预测和当前状态的不确定性。卡尔曼滤波器还可以预测以下状态依此类推。 下图提供了卡尔曼滤波操作的完整图片。 下表描述了所有卡尔曼滤波方程。 下表总结了符号包括文献中发现的差异和尺寸。 尺寸表示法 \ 是状态向量中的多个状态\是许多测量状态\是输入变量的多个元素 例子 这是多元卡尔曼滤波章节的最后一部分。 它包括两个数值示例。在第一个例子中我们设计了一个没有控制输入的六维卡尔曼滤波器。在第二个示例中我们设计了一个带有控制输入的二维卡尔曼滤波器。 示例 9 – 车辆位置估计 在下面的示例中我们使用所学的材料实现多元卡尔曼滤波器。 在此示例中我们想估计车辆在 XY 平面上的位置。 车辆有一个车载位置传感器可报告系统的 \ X \ 和 \ Y \ 坐标。 我们假设恒定的加速度动态。 状态外推方程 首先我们推导出状态外推方程。您还记得矩阵表示法中状态外推方程的一般形式是 \是时间步长 \ n 1 \ 处的预测系统状态向量\是时间步长 \ n \ 处的估计系统状态向量\是一个控制变量\是过程噪声\F是状态转移矩阵\G是控制矩阵 在此示例中没有控制变量 因为没有控制输入。 对于此示例状态外推方程可以简化为 系统状态由下式定义 时间 \ n 1 \ 的外推车辆状态可以用以下方程组来描述 以矩阵形式 协方差外推方程 协方差外推方程的一般形式由下式给出 Pnn是当前状态估计的协方差矩阵Pn1n是下一个状态估计预测的协方差矩阵F是我们在“线性动态系统建模”部分中推导的状态转换矩阵Q是过程噪声矩阵 估计协方差矩阵为 矩阵主对角线上的元素是估计的方差 是 X 坐标位置估计的方差pxX 是 X 坐标速度估计的方差px˙˙X是 X 坐标加速度估计的方差px¨X是 Y 坐标位置估计的方差pyY 是 Y 坐标速度估计的方差py˙Y是 Y 坐标加速度估计的方差py¨Y非对角线条目是协方差 我们假设 X 轴和 Y 轴中的估计误差不相关因此互项可以设置为零。 我们已经导出了状态转换矩阵  F 。现在我们将推导出过程噪声 Q 矩阵。 过程噪声矩阵 我们假设一个离散噪声模型 - 每个时间样本的噪声不同但在时间样本之间是恒定的。 二维恒加速度模型的过程噪声矩阵如下所示 我们已经推导出了恒加速度运动模型的 Q 矩阵。我们示例中的 Q 矩阵为 其中 是连续测量之间的时间 是加速度的随机方差 现在我们可以为我们的示例写下协方差外推方程 测量公式 矩阵形式的广义测量方程由下式给出 zn是测量向量xn是真实的系统状态隐藏状态vn随机噪声矢量H是观察矩阵 测量仅为我们提供车辆的 X 和 Y 坐标。So ​​​​​​​        ​​​​​​​        ​​​​​​​         的维度是 2X1的维度是 。因此观察矩阵 的维数应为 2 X 6 。 测量不确定度 卡尔曼增益 矩阵符号中的卡尔曼增益由下式给出 我们已经推导出了卡尔曼增益的所有构建块 状态更新公式 矩阵形式的状态更新方程由下式给出 我们已经定义了状态更新方程的所有构建块。 协方差更新方程 协方差更新方程由下式给出 我们已经定义了协方差更新方程的所有构建块。 数值示例 现在我们准备求解数值示例。让我们假设一辆车辆以恒定的速度沿 X 方向直线移动。行驶400米后车辆右转转弯半径为300米。在转弯操作期间车辆会因圆周运动角加速度而加速。 下图描述了车辆的运动。 ​​​​​​​         测量周期 Δt1s随机加速度标准差 \ 测量误差标准差 状态转换矩阵  F  将为 过程噪声矩阵 Q 将为 测量协方差  R 将为 下表包含一组 35 个噪声测量值 迭代零 初始化 我们不知道车辆位置;我们将初始位置、速度和加速度设置为 0 由于我们的初始状态向量是一个猜测因此我们设置了一个非常高的估计不确定性。高估计不确定度通过为测量提供高权重而导致高卡尔曼增益。 预测 现在我们可以根据初始化值预测下一个状态。 第一次迭代 步骤 1 - 测量 测量值 步骤 2 - 更新 卡尔曼增益计算 如您所见位置的卡尔曼增益为 0.9921这意味着第一次测量的权重明显高于估计的权重。估计的权重可以忽略不计。 估计当前状态 更新当前估计协方差 步骤 3 - 预测 我们的预测不确定性仍然很高。 第二次迭代 步骤 1 - 测量 测量值  ​​​​​​​        ​​​​​​​        ​​​​​​​         步骤 2 - 更新 卡尔曼增益计算 估计当前状态 更新当前估计协方差 步骤 3 - 预测 我们的预测不确定性仍然很高。 此时跳转到最后一个卡尔曼滤波迭代是合理的。 第三十五次迭代 步骤 1 - 测量 测量值 ​​​​​​​         步骤 2 - 更新 卡尔曼增益计算 该位置的卡尔曼增益已收敛到 0.56这意味着测量和估计权重几乎相等。 估计当前状态 更新当前估计协方差 此时位置方差  这意味着估计的标准差为 。 步骤 3 - 预测 示例分析 下图显示了卡尔费休的位置和速度估计性能。 左侧的图表比较了车辆位置的真实值、测量值和估计值。右侧的两个图表比较了 x 轴速度和 y 轴速度的真实值、测量值和估计值。 如您所见卡尔曼滤波器成功跟踪车辆。 让我们放大车辆运动的线性部分和转弯机动部分。 图上的圆圈表示 95% 置信度椭圆。由于 x 轴和 y 轴的测量误差相等因此置信度椭圆是一个圆。 当车辆沿直线行驶时加速度恒定且等于零。然而在转弯机动期间由于圆周运动 - 角加速度车辆会经历加速。 回想一下物理学入门角加速度是  其中是时间间隔\Delta V 是时间间隔内的速度差R 是圆半径。 尽管角加速度是恒定的但 x 轴和 y 轴上的角加速度投影不是恒定的因此 和  不是恒定的。  我们的卡尔曼滤波器专为恒加速度模型而设计。尽管如此由于正确选择了 参数它成功地跟踪了机动车辆。         我想鼓励读者在软件中实现这个例子看看或的不同值如何影响实际的卡尔曼滤波精度、卡尔曼增益收敛性和估计不确定性。 示例 10 – 火箭高度估计 在此示例中我们估计火箭的高度。火箭配备了一个提供高度测量的机载高度计。除了高度计外火箭还配备了一个加速度计用于测量火箭的加速度。 加速度计用作卡尔曼滤波器的控制输入。 我们假设恒定的加速度动态。 ​​​​​​​        ​​​​​​​         加速度计不感知重力。静止在桌子上的加速度计向上测量 1g而自由落体的加速度计测量零加速度。因此我们需要从每个加速度计测量中减去重力加速度常数g。 时间步长 n 的加速度计测量值为 是对象的实际加速度对象位置的二阶导数g是重力加速度常数; g -9.8    ε 是加速度计测量误差 状态外推方程 矩阵表示法中状态外推方程的一般形式为 在这个例子中我们有一个控制变量  u  它基于加速度计的测量。 系统状态由下式定义 ​​​​​​​         是时间 n 的火箭高度是时间 n 的火箭速度 我们可以将状态外推方程表示如下 在上式中 协方差外推方程 协方差外推方程的一般形式为 矩阵形式的估计协方差为 矩阵主对角线的元素是估计的方差 是高度估计的方差 是速度估计的方差非对角线条目是协方差 我们已经导出了状态转换矩阵  F。现在我们将推导出过程噪声 Q 矩阵。 过程噪声矩阵 我们假设一个离散噪声模型 - 每个时间样本的噪声不同但在时间样本之间是恒定的。 恒加速度模型的过程噪声矩阵如下所示 是连续测量之间的时间 是加速度计测量中的随机方差 在前面的示例中我们使用系统的加速度随机方差  作为过程噪声矩阵的乘数。但在这里我们有一个加速度计来测量系统的随机加速度。加速度计误差v远低于系统的随机加速度;因此我们使用   作为过程噪声矩阵的乘数。 它使我们的估计不确定性大大降低 现在我们可以为我们的示例写下协方差外推方程 注意根据“构造过程噪声矩阵”一章恒速模型的 Q 矩阵的大小应为2 X 2恒加速度模型的Q 矩阵的大小应为3 X 3。 在此示例中加速度由控制输入处理因此它不是 F 矩阵的一部分并且 Q 矩阵为 2 X 2 。 如果没有外部控制输入过程矩阵将为 2 X 2 。 测量公式 矩阵形式的广义测量方程由下式给出 ​​​​​​​        ​​​​​​​         测量仅提供火箭的高度 ​​​​​​​         的维数是 而 的维数是 所以观察矩阵的维数是 。  测量不确定度 卡尔曼增益 矩阵符号中的卡尔曼增益由下式给出 我们已经推导出了卡尔曼增益的所有构建块         状态更新公式 矩阵形式的状态更新方程由下式给出 ​​​​​​​         ​​​​​​​         我们已经定义了状态更新方程的所有构建块。 协方差更新方程 协方差更新方程由下式给出 我们已经定义了协方差更新方程的所有构建块。 数值示例 让我们假设一个具有恒定加速度的垂直助推火箭。火箭配备了一个提供高度测量的高度计和一个用作控制输入的加速度计。 测量周期  Δt0.25s火箭加速 高度计测量误差标准偏差加速度计测量误差标准偏差 状态转换矩阵 将为 控制矩阵 将是 过程噪声矩阵 将为 测量方差为 下表包含一组高度计和加速度计的 30 个噪声测量值 迭代零 初始化 我们不知道火箭的位置;我们将初始位置和速度设置为 0。 我们也不知道火箭的加速度但我们可以假设它大于零。让我们假设 由于我们的初始状态向量是一个猜测因此我们设置了一个非常高的估计不确定性。高估计不确定性导致高卡尔曼增益从而为测量提供高权重。 预测 现在我们可以根据初始化值预测下一个状态。 第一次迭代 步骤 1 - 测量 测量值 步骤 2 - 更新 卡尔曼增益计算 估计当前状态 更新当前估计协方差 我们的预测不确定性仍然很高。 第二次迭代 步骤 1 - 测量 测量值 步骤 2 - 更新 卡尔曼增益计算 估计当前状态 更新当前估计协方差 步骤 3 - 预测 此时跳转到最后一个卡尔曼滤波迭代是合理的。 第三十次迭代 步骤 1 - 测量 测量值 ​​​​​​​        ​​​​​​​         步骤 2 - 更新 卡尔曼增益计算 高度的卡尔曼增益收敛到0.12这意味着估计权重远高于测量权重。 估计当前状态 更新当前估计协方差 此时高度方差 这意味着估计的标准差为  请记住测量的标准差为 20m。 步骤 3 - 预测 示例分析 下图比较了火箭高度的真实值、测量值和估计值。 ​​​​​​​         我们可以看到良好的钦哲基金会跟踪性能和收敛性。 下图比较了火箭速度的真实值、测量值和估计值。 将估计值收敛到真正的火箭速度大约需要 2.5 秒。 一开始估计的高度受到测量值的影响并且由于测量值非常嘈杂因此与真实的火箭高度不太一致。 但随着KF收敛噪声测量的影响较小估计的高度与真实高度完全一致。 在这个例子中我们没有任何导致加速度变化的动作但如果我们有控制输入加速度计将更新状态外推方程。
http://www.dnsts.com.cn/news/271837.html

相关文章：

  • 网站遮罩是什么游戏软件开发公司排名
  • 如何实现企业网站推广的系统性个人社保缴费基数是什么意思
  • 加强普法网站建设的通知物联网卡一年服务费多少钱啊
  • 短视频分享网站开发做电影网站用什么程序
  • 公众号版影视网站开发国外画册设计网站
  • 广州找人做网站whois域名查询官网
  • 做门户网站服务器选择石狮住房和城乡建设网站
  • 品牌网站的目的做电商网站的设计思路有什么意思
  • 山东省环保厅官方网站建设项目网站建设超速云免费
  • 湖南吉首建设官方网站公司网站开发的流程
  • 网站运营目标网站上的地图代码
  • 阳江市建设网站定制产品
  • 广州网站建设 骏域网站建设专家什么网站系统做的最好
  • 重庆网站推广系统如何给网站做排名
  • 微网站是什么网站建设中界面模板下载
  • 网站设计制作自己怎么做一个企业官网
  • 深圳专业做网站设计公司wordpress视频播放卡
  • 建设网站为什么要虚拟主机wordpress adsense integrator
  • 网站开发 行业动态wordpress文章字体大小
  • 网络网站销售私域电商平台
  • 网站建设汇报书 ppt替换wordpress管理路径
  • 成都市住房和城乡建设部网站用群晖建wordpress
  • 个人网站备案网站名称网站开发的论文课题
  • 手机网站 焦点图怎么修改wordpress
  • 苏宁网站建设一个好的网站应该具有什么
  • 怎么免费制作网站娄底seo排名
  • 网站地图做法做天津建设工程
  • 网站维护提示页面模板抖音代运营一般哪家好
  • 网站建设前wordpress源码书籍
  • 能制作网站的公司联系方式wordpress 主题更改