CN105956617B - 一种针对类自行车运动的人车姿态联合估计方法 - Google Patents

Abstract

本发明公开了一种针对类自行车运动的人车姿态联合估计方法，对代表人车系统的刚体‑移动运动平台系统进行运动学建模，利用两个角速度计和补偿传感器，推导出融合算法的特殊系统方程和观测方程，最终使用拓展型卡尔曼滤波器实现传感器融合，同时得到无漂的刚体姿态估计和移动平台相对世界坐标系的无漂绝对倾斜角估计，解决了现有融合技术下获得绝对姿态估计必须需要绝对姿态运动测量或者基准捕捉的问题。

Description

一种针对类自行车运动的人车姿态联合估计方法

技术领域

本发明属于传感器融合技术领域，涉及一种针对类自行车运动的人车姿态联合估计方法。

背景技术

对人体运动姿态的精确估计无论对运动缺失病人的定量诊断，还是专业运动员的训练等方面都十分重要。在人的各种运动中，有很大一部分运动都是属于户外物理人机交互系统，如骑自行车、划皮划艇等。这些物理人机交互系统一般可以被抽象为刚体和移动运动平台的联合系统。

对于户外运动的姿态估计，常见的室内光学运动捕捉系统显然无法适用。基于MEMS技术的可穿戴式惯性传感器，如角速度计等被广泛应用在人体户外姿态估计中。利用惯性角速度计进行姿态估计的主要问题是其只能测量角速度，因此为了得到姿态信息还必须进行积分。而由于测量信号中的随机游走漂移噪声和白噪声等，信号积分会产生严重的积分漂移问题。为了克服这个问题，一般姿态估计方案通常还会使用补偿传感器(比如加速度计，磁力传感器，倾角仪，超声传感器，本地车载相机等)与角速度计传感器进行融合。补偿传感器一般低性能，有低精度或者低频率，但是无漂的特点。角速度计虽然具有高精度高动态特点，但积分有严重漂移问题。通过传感器融合技术可综合多种传感器的优点。角速度计与补偿传感器融合后的姿态估计一般会同时具有无漂且高精度的特性。普遍认为，为了通过惯性传感器如角速度计融合获得无漂的高精度全局姿态估计(相对于世界绝对坐标系)，惯性传感器和补偿传感器中至少需要有一个传感器必须能够测量世界坐标系下绝对运动信息或者捕捉到绝对姿态基准，如加速度计可以测得重力向量，磁力传感器可以测得地磁场。仅仅融合本地相对运动测量的传感器(如融合角速度计和超声传感器)只能够消除本地相对姿态估计(如人体关节角度)的漂移，但对世界坐标下的绝对姿态估计仍然会有明显的漂移问题。

发明内容

本发明的目的是提供一种针对类自行车运动的人车姿态联合估计方法，以解决现有融合技术下获得绝对姿态估计必须需要绝对姿态运动测量或者基准捕捉的问题。

本发明所采用的技术方案是，一种针对类自行车运动的人车姿态联合估计方法，对代表人车系统的刚体-移动运动平台系统进行运动学建模，利用两个角速度计和补偿传感器，推导出融合算法的特殊系统方程和观测方程，最终使用拓展型卡尔曼滤波器实现传感器融合，同时得到无漂的刚体姿态估计和移动平台相对世界坐标系的无漂绝对倾斜角估计。

进一步的，具体按照以下步骤实施：

步骤1、在移动运动平台和刚体上分别安装角速度计，在移动运动平台上安装补偿传感器，并对刚体和移动运动平台进行系统配置和系统变量定义；

步骤2、对刚体和移动运动平台系统进行建模，并推导融合算法的特殊系统方程和观测方程：

系统方程，包括关于刚体姿态角θ的运动学方程，移动运动平台绝对倾斜角的运动学方程，移动运动平台角速度计测量噪声中的随机游走噪声b^p和刚体角速度计测量噪声中的随机游走噪声b^h；

观测方程，描述了θ到补偿传感器测量值之间的线性关系；

步骤3、对步骤2中系统方程和观测方程进行整理并离散化，得到k时刻的离散系统方程和观测方程，再通过拓展型卡尔曼滤波器公式得到无漂的刚体姿态估计和移动运动平台相对世界坐标系的无漂倾斜角估计。

进一步的，步骤1中，对刚体和移动运动平台进行系统配置和系统变量定义具体为：

对刚体和移动运动平台分别定义两个坐标系B_H(x_h,y_h,z_h)和B_P(x_p,y_p,z_p)，定义φ和ψ分别表示移动运动平台相对于惯性坐标系I的倾斜角、俯仰角和偏航角，定义B_P和B_H之间的欧拉角θ是刚体相对于移动运动平台的姿态角，安装在移动运动平台上的角速度计1的测量值定义为安装在刚体上的三轴角速度计1的测量值定义为

进一步的，步骤2中，建立角速度计在移动运动平台的本体坐标系下的角速度模型，如下：

其中，代表围绕i轴且旋转角度为的旋转矩阵，其中i(i＝x,y,z)，定义 代表了移动平台真实运动角速度，向量v^p和向量b^p分别代表移动运动平台上的角速度计所测量噪声中的零均值白噪声和随机游走噪声，且

假设b^p满足一阶马尔可夫模型，即表示为：

其中，u^p为移动运动平台角速度计随机游走噪声的驱动白噪声，

和均为时间常数，diag表示对角矩阵；

根据公式(1)求解得到移动运动平台的一般运动学模型，如下:

提取公式(1)中的第1行元素项和第3行元素项并求解，得到针对类自行车移动运动的特殊运动学模型，如下：

由公式(4)可得类自行车运动的倾斜角运动学方程，如下：

进一步的，步骤2中，建立刚体运动的角速度计模型，如下：

其中，代表了刚体真实运动角速度，向量v^h和向量b^h分别代表刚体上的角速度计所测量噪声中的零均值白噪声和随机游走噪声，且

b^h为一阶马尔可夫模型，则表示为：

其中，u^h为刚体角速度计随机游走噪声的驱动白噪声， 和均为时间常数，diag表示对角矩阵；

将公式(6)的最后两项合并，再将和用公式(4)中的两项分别替换，则公式(6)的最后两项合并为：

由公式(8)和公式(6)联合求解关于刚体姿态角θ的运动学方程，即为：

进一步的，步骤2中的观测方程具体为：

y＝θ+v^y (10)，

其中，v^y是观测高斯白噪声，y是补偿传感器真实测量值，

进一步的，步骤3中，采用拓展型卡尔曼滤波器，将k时刻的系统状态向量定义为：

k时刻，刚体和移动运动平台的角速度计的实际测量值分别表示为和然后，将移动运动平台角速度计测量噪声中的随机游走噪声b^p、刚体角速度计测量噪声中的随机游走噪声b^h、针对移动运动平台的倾斜角运动学方程、以及关于刚体姿态角θ的运动学方程的离散形式重新组织为：

其中，T是离散时间间隔，A(x_k)和B(x_k)是两个非线性系数矩阵，

系统k时刻的观测值为则k时刻的观测方程为：

其中，是k时刻的观测高斯白噪声向量。其中N(0,R_y)表示以0为均值，R_y为协方差的高斯分布,diag表示对角矩阵。

进一步的，步骤3通过拓展型卡尔曼滤波器实现传感器融合的具体方法是使用典型EKF滤波五个公式来实现，具体为：

1)第一个公式为状态转移方程，目的是得到EKF中状态预测

其中是第k步的EKF状态最优估计，代表进行了一步状态转移后的状态预测估计值,F_k是公式(12)的一阶泰勒展开系数，下同；

2)第二个公式得到状态估计误差协方差P_k+1|k的转移方程，

P_k+1|k＝F_kP_k|kF_k ^T+Q_k，其中P_k|k是第k步误差协方差估计，P_k+1|k是进行了一步状态转以后的误差协方差估计，Q_k为过程噪声方差矩阵；

3)第三个公式为卡尔曼增益K_k+1计算，

K_k+1＝P_k+1|kH^T[HP_k+1|kH^T+R_k+1]^-1，

其中

4)第四个公式为滤波估计方程，根据观测值的状态更新，得到第k+1步的EKF状态最优估计

5)第五个是状态估计误差协方差矩阵更新方程，P_k+1|k+1＝[I-K_k+1H]P_k+1|k，其中P_k+1|k+1是第k+1步的误差协方差估计更新，I是单位矩阵。

本发明的有益效果是，使用两个角速度计和其相对姿态测量(通过补偿传感器)的人车系统姿态联合估计方法，在没有使用任何绝对姿态信息的情况下，就可以同时获得人体的无漂姿态角估计和移动运动平台的无漂全局绝对倾斜角估计。

附图说明

图1是本发明人车姿态联合估计方法中类自行车运动的刚体-移动运动平台的系统结构示意图；

图2是本发明人车姿态联合估计方法中传感器融合系统的原理图。

图中，1.角速度计，2.补偿传感器，H.刚体，P.移动运动平台。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明的主要特征是通过基于MEMS的双角速度计和其相对姿态测量(如通过相机、超声传感器等补偿传感器)融合滤波，针对类似自行车运动、皮划艇等户外物理人机交互运动的一系列刚体-移动运动平台系统进行姿态联合估计。图1是这一系列系统的结构示意图。系统由移动运动平台P和在其上运动的刚体H组成(如人体躯干)。刚体H和各装载一个角速度计1测量各自的角速度信息，同时有一个低性能补偿传感器2可以测量刚体H和移动运动平台P之间的相对姿态。很明显，这样的传感器配置方法只能测量本地相对运动信息。本发明提出的方法可以通过以上的传感器配置，同时获得刚体H相对平台的无漂移姿态估计以及平台相对世界坐标系的无漂倾斜角估计。实现本发明提出的姿态估计方法主要有以下三个步骤：

步骤1、首先进行系统配置和系统变量等定义：

图1是本发明针对类似人车系统的结构示意图。H和P分别代表刚体(如人体躯干)和移动运动平台。同时，我们对刚体H和移动运动平台P分别定义两个体坐标系B_H(x_h,y_h,z_h)和B_P(x_p,y_p,z_p)。我们定义φ和ψ分别表示移动运动平台P相对于惯性坐标系I的倾斜角(沿着x_p转动)、俯仰角(沿着y_p转动)和偏航角(沿着z_p转动)。从惯性坐标系到B_P是依照偏航角、俯仰角、倾斜角的顺序通过欧拉角变化旋转得到。再定义B_P和B_H之间的欧拉角θ＝[θ₁ θ₂θ₃]^T(通过X-Y-Z旋转顺序)，因此θ是刚体H的姿态角(相对于车体)。

在传感器配置方面，我们将两个角速度计1分别安装在移动运动平台P和刚体H负载上，如图1所示，角速度计1通常选择三轴角速度计。其测量值分别定义为和刚体姿态角θ可由一个补偿传感器2(如相机或超声传感器)直接测量到。补偿传感器2一般有低精度或者低频率，但无漂的特点。

步骤2、在步骤1的基础上，对刚体-移动平台系统进行运动学建模，并推导出传感器融合算法的特殊系统方程和观测方程：由步骤1定义，我们建立角速度计在移动运动平台P本体坐标系下的平台角速度模型：

其中，代表围绕i轴且旋转角度为的旋转矩阵，其中i(i＝x,y,z)，定义 (下文均采用类似简写)。代表了移动平台真实运动角速度，向量v^p和向量b^p分别代表移动运动平台(P)上的角速度计(1)所测量噪声中的零均值白噪声和随机游走噪声，且 b^p假设满足一阶马尔可夫模型，可表示为

其中u^p为平台角速度计随机游走噪声的驱动白噪声， 为时间常数，diag表示对角矩阵。

我们根据公式(1)求解可计算得移动运动平台P的一般运动学模型:

显然，直接对公式(3)进行积分得到移动运动平台绝对姿态会有严重的积分漂移。

对自行车运动或者类似运动，我们可以假设零俯仰角运动，也就是充分利用这个运动约束是本发明的关键。对于零俯仰角运动，我们可以提取公式(1)中的第1行元素项和第3行元素项并求解，得到针对类自行车移动运动的运动学模型

由上式可得类移动运动平台P的运动倾斜角运动学方程：

类似于移动运动平台P上的角速度计1，建立刚体运动模型的角速度计模型为

其中，代表了刚体真实运动角速度向量，和分别代表陀螺仪测量噪声中的零均值白噪声和随机游走噪声。b^h假设为一阶马尔可夫模型，可表示为

且其中，u^h为刚体角速度计随机游走噪声的驱动白噪声， 为时间常数，diag表示对角矩阵。消除公式(6)中的和我们可以得到刚体运动学模型。但是注意这里有两种消除方法。

方法1：公式(6)的最后两项可以合并，并直接被公式(1)替换(不考虑俯仰角运动学)，也就是最后两项可以变为

方法2：公式(6)的最后两项合并，其中和被公式(4)中的两项分别替换，这样最后两项可以变为

在本发明里，必须使用方法2，否则不能得到预期的效果。由方法2和公式(6)，联合求解关于θ的运动学方程，得到

注意这里可以看到公式(8)是含有平台倾斜角的，而利用方法1得到的运动学方程不含因此不能得到对倾斜角的无漂估计效果。

由于θ是补偿传感器2直接测量得到的，因此有

y＝θ+v^y (11)，

其中，是观测方程中的高斯白噪声，y＝[y¹ y² y³]^T是补偿传感器2真实测量值。

图2表明了本发明融合算法的结构。将公式(2)、(5)、(7)、(10)作为融合算法的系统方程，将公式(11)看作观测方程。然后可以使用例如卡尔曼滤波等一系列实际滤波器对融合进行实现。

步骤3、利用步骤2得到的系统方程和观测方程，具体使用拓展型卡尔曼滤波器(EKF)实现传感器融合，最终同时得到无漂的刚体姿态估计和移动运动平台P相对世界坐标系的无漂倾斜角估计。

本发明通过拓展型卡尔曼滤波公式得到无漂的刚体姿态估计和移动运动平台P相对世界坐标系的无漂倾斜角估计。所述本发明传感器融合算法收敛的条件是满足即自行车移动运动平台P的偏航角速度不能绝对为零，具体详细证明见说明书。这个条件在实际运用中由于脚踏效应、转向、干扰等因素，很容易满足。

本步骤我们使用拓展性卡尔曼滤波器(EKF)对步骤2得到的融合算法进行实现。考虑滤波器的离散形式，k时刻的系统状态向量定义为：

k时刻刚体H和移动运动平台P角速度计实际测量值分别表示为和这里并不考虑因为公式(9)中没有的y轴分量。系统方程的离散形式可以重新组织为：

其中T是离散时间间隔，A(x_k)和B(x_k)是两个非线性系数矩阵：

系统k时刻的观测值为观测方程写作：

其中，是k时刻的观测高斯白噪声向量。其中N(0,R_y)表示以0为均值，R_y为协方差的高斯分布，diag表示对角矩阵。

下面使用典型EKF滤波五个公式实现：

第一个公式为状态转移方程，目的是得到EKF中状态预测

其中是第k步的EKF状态最优估计，代表进行了一步状态转移后的状态预测估计值,F_k是公式(12)的一阶泰勒展开系数：

和diag表示对角矩阵。

F₁中F_ij的值为：

第二个公式得到状态估计误差协方差P_k+1|k的转移方程，

P_k+1|k＝F_kP_k|kF_k ^T+Q_k，其中P_k|k是第k步误差协方差估计，P_k+1|k是进行了一步状态转以后的误差协方差估计，Q_k为过程噪声方差矩阵，一般看作可调参数。

第三个公式为卡尔曼增益K_k+1计算，控制收敛速度。

K_k+1＝P_k+1|kH^T[HP_k+1|kH^T+R_k+1]^-1，在本发明中，

第四个公式为滤波估计方程，根据观测值的状态更新，得到第k+1步的EKF状态最优估计

第五个是状态估计误差协方差矩阵更新方程，P_k+1|k+1＝[I-K_k+1H_k+1]P_k+1|k，其中P_k+1|k+1是第k+1步的误差协方差估计更新，I是单位矩阵。

通过以上五个公式，就可以同时获得人体的无漂姿态角估计和移动运动平台的无漂全局绝对倾斜角估计。注意为了达到本发明期望的对平台绝对倾斜角的无漂估计效果，还需要满足条件，即自行车移动运动平台P的偏航角速度不能绝对为零，详细证明见随后EKF误差分析。这种条件在日常中是很容易满足的，比如任何的蹬踏板，拐弯，振动等都可以实现偏航角速度不绝对为零。即使在某一段时间，系统的偏航角速度绝对为零，仅意味着误差会变大。但一旦偏航角速度再为零，误差会立即快速归零，所以本发明提供的方法通过验证是可以满足对平台绝对倾斜角估计误差短时有界，等效于无漂估计的。

利用EKF误差分析来对本发明公开的对平台倾斜角进行无漂估计进行理论证明。令表示EKF中x(k)的状态估计。定义估计误差：和其中e_x(k+1|k)和e_x(k+1|k+1)分别表示预测和更新后的误差。将系统方程公式(12)在x处泰勒展开并取线性近似，我们得到预测误差

e_x(k+1|k)＝(I+TF(k))e_x(k|k) (14)，

I是单位矩阵。

再推导误差更新模型。结合e_x(k+1|k)和e_x(k+1|k+1)，我们有

EKF的更新方程为

其中K(k)表示更新增益矩阵。将(16)代入(15)，误差更新表示为

e_x(k+1|k+1)＝[I-K(k)H(k)]e_x(k+1|k)-Kv^y(k) (17)，

合并公式(17)和公式(14)，我们得到完整的误差模型

e_x(k+1|k+1)＝E(k)e_x(k|k)-K(k)v^y(k) (18)，

其中E(k)＝[I-K(k)H(k)][I+TF(k)]。对于本发明提出的融合方案，很容易验证当时，E(k)-I是满秩的。这意味着在这种情况下，可以通过分配合适的增益使得系统的极点都在左半复平面，从而使得所有误差收敛为零，包括误差因此，的运动误差在时是随着误差收敛的，即能够得到无漂移的滤波结果。

本发明的目的是公开一种针对类似自行车运动等户外人机交互系统，利用双角速度计和其相对姿态测量的完全本地运动信息，实现一种不仅能获得人体无漂姿态估计而且还能获得部分车体全局绝对姿态的姿态联合估计方法。现有的传感器融合技术一般需要具有能够测量绝对姿态或者绝对基准的补偿传感器2，才能消除惯性角速度计针对全局估计的积分漂移问题。因此，相对于现有技术，本发明是一种明显的改进，突破了现有技术的局限性。

本发明方法实现的关键是利用类似自行车等运动的运动约束，设计特殊的运动学方程，将车体绝对倾斜角耦合到传感器融合的系统方程当中，从而使得刚体H与移动运动平台P之间的相对姿态测量能对车体的绝对倾斜角进行漂移修正。

Claims (7)

1.一种针对类自行车运动的人车姿态联合估计方法，其特征在于，对代表人车系统的刚体-移动运动平台系统进行运动学建模，利用两个角速度计(1)和补偿传感器(2)，推导出融合算法的特殊系统方程和观测方程，最终使用拓展型卡尔曼滤波器实现传感器融合，同时得到无漂的刚体姿态估计和移动平台相对世界坐标系的无漂绝对倾斜角估计；

具体按照以下步骤实施：

步骤1、在移动运动平台(P)和刚体(H)上分别安装角速度计(1)，在移动运动平台(P)上安装补偿传感器(2)，并对刚体(H)和移动运动平台(P)进行系统配置和系统变量定义；

步骤2、对刚体(H)和移动运动平台(P)系统进行建模，并推导融合算法的特殊系统方程和观测方程；

所述系统方程，包括关于刚体姿态角θ的运动学方程，移动运动平台绝对倾斜角的运动学方程，移动运动平台角速度计测量噪声中的随机游走噪声b^p和刚体角速度计测量噪声中的随机游走噪声b^h；

所述观测方程，描述了θ到补偿传感器测量值之间的线性关系；

步骤3、对步骤2中系统方程和观测方程进行整理并离散化，得到k时刻的离散系统方程和观测方程，再通过拓展型卡尔曼滤波器公式得到无漂的刚体姿态估计和移动运动平台(P)相对世界坐标系的无漂倾斜角估计。

2.如权利要求1所述的一种针对类自行车运动的人车姿态联合估计方法，其特征在于，所述步骤1中，对刚体(H)和移动运动平台(P)进行系统配置和系统变量定义具体为：

对刚体(H)和移动运动平台(P)分别定义两个坐标系B_H(x_h,y_h,z_h)和B_P(x_p,y_p,z_p)，定义φ和ψ分别表示移动运动平台(P)相对于惯性坐标系I的倾斜角、俯仰角和偏航角，定义B_P和B_H之间的欧拉角θ是刚体(H)相对于移动运动平台(P)的姿态角，安装在移动运动平台(P)上的角速度计(1)的测量值定义为安装在刚体(H)上的三轴角速度计(1)的测量值定义为

3.如权利要求1所述的一种针对类自行车运动的人车姿态联合估计方法，其特征在于，所述步骤2中，建立角速度计在移动运动平台(P)的本体坐标系下的角速度模型，如下：

其中，代表围绕i轴且旋转角度为的旋转矩阵，其中i(i＝x,y,z)，定义 代表了移动平台真实运动角速度，向量v^p和向量b^p分别代表移动运动平台(P)上的角速度计(1)所测量噪声中的零均值白噪声和随机游走噪声，且

假设b^p满足一阶马尔可夫模型，即表示为：

其中，u^p为移动运动平台角速度计随机游走噪声的驱动白噪声，和均为时间常数，diag表示对角矩阵，b^p为随机游走噪声，表示随机游走噪声的导数，

根据公式(1)求解得到移动运动平台(P)的一般运动学模型，如下:

其中，为倾斜角的角速度，为俯仰角的角速度，为偏航角的角速度；

提取公式(1)中的第1行元素项和第3行元素项并求解，得到针对类自行车移动运动的特殊运动学模型，如下：

由公式(4)可得类自行车运动的倾斜角运动学方程，如下：

4.如权利要求3所述的一种针对类自行车运动的人车姿态联合估计方法，其特征在于，所述步骤2中，建立刚体运动的角速度计模型，如下：

其中，代表了刚体真实运动角速度，向量v^h和向量b^h分别代表刚体(H)上的角速度计(1)所测量噪声中的零均值白噪声和随机游走噪声，且b^h为一阶马尔可夫模型，则表示为：

其中，u^h为刚体角速度计随机游走噪声的驱动白噪声， 和均为时间常数，diag表示对角矩阵；

将公式(6)的最后两项合并，再将和用公式(4)中的两项分别替换，则公式(6)的最后两项合并为：

由公式(8)和公式(6)联合求解关于刚体姿态角θ的运动学方程，即为：

其中，表示θ₁的角速度，表示θ₂的角速度，表示θ₃的角速度。

5.如权利要求4所述的一种针对类自行车运动的人车姿态联合估计方法，其特征在于，所述步骤2中的观测方程具体为：

y＝θ+v^y (10)，

其中，v^y是观测高斯白噪声，y是补偿传感器真实测量值，y＝[y¹ y²y³]^T。

6.如权利要求1所述的一种针对类自行车运动的人车姿态联合估计方法，其特征在于，所述步骤3中，采用拓展型卡尔曼滤波器，将k时刻的系统状态向量定义为：

k时刻，刚体(H)和移动运动平台(P)的角速度计(1)的实际测量值分别表示为和然后，将移动运动平台角速度计测量噪声中的随机游走噪声b^p、刚体角速度计测量噪声中的随机游走噪声b^h、针对移动运动平台的倾斜角运动学方程、以及关于刚体姿态角θ的运动学方程的离散形式重新组织为：

其中，T是离散时间间隔，A(x_k)和B(x_k)是两个非线性系数矩阵，

系统k时刻的观测值为则k时刻的观测方程为：

其中，是k时刻的观测高斯白噪声向量，其中N(0,R_y)表示以0为均值，R_y为协方差的高斯分布,diag表示对角矩阵。

7.如权利要求6所述的一种针对类自行车运动的人车姿态联合估计方法，其特征在于，所述步骤3通过拓展型卡尔曼滤波器实现传感器融合的具体方法是使用典型EKF滤波五个公式来实现，具体为：

1)第一个公式为状态转移方程，目的是得到EKF中状态预测

其中是第k步的EKF状态最优估计，代表进行了一步状态转移后的状态预测估计值,F_k是公式(12)的一阶泰勒展开系数，下同；

2)第二个公式得到状态估计误差协方差P_k+1|k的转移方程，

P_k+1|k＝F_kP_k|kF_k ^T+Q_k，其中P_k|k是第k步误差协方差估计，P_k+1|k是进行了一步状态转以后的误差协方差估计，Q_k为过程噪声方差矩阵；

3)第三个公式为卡尔曼增益K_k+1计算，

K_k+1＝P_k+1|kH^T[HP_k+1|kH^T+R_k+1]^-1，

其中

H为测量方程雅克比矩阵；

4)第四个公式为滤波估计方程，根据观测值的状态更新，得到第k+1步的EKF状态最优估计

5)第五个是状态估计误差协方差矩阵更新方程，P_k+1|k+1＝[I-K_k+1H]P_k+1|k，其中P_k+1|k+1是第k+1步的误差协方差估计更新，I是单位矩阵。