CN106500695A - 一种基于自适应扩展卡尔曼滤波的人体姿态识别方法 - Google Patents

Abstract

本发明公开了一种基于自适应扩展卡尔曼滤波的人体姿态识别方法，属于体域网领域。该方法分为两个部分，模型设计和参数设计。模型设计中，利用四元数可反映人体肢体运动角度的特点，通过惯性传感器采集人体运动的角速度、加速度和周边磁场强度，基于自适应扩展卡尔曼滤波方法进行姿态解算，求得姿态四元数。参数设计中，通过理论分析和实验方法确定了过程噪声协方差矩阵、测量噪声协方差矩阵的取值，以及状态初始值和状态协方差矩阵初始值，使自适应扩展卡尔曼滤波方法可以持续迭代进行，从而不断实时识别人体运动姿态。本发明可作为体育训练、医疗保健、游戏设计等领域的人体姿态识别方法使用。

Description

一种基于自适应扩展卡尔曼滤波的人体姿态识别方法

技术领域

本发明属于体域网领域，涉及一种基于自适应扩展卡尔曼滤波的人体姿态识别方法。

背景技术

人体姿态识别可以捕捉到某一时刻人体的空间运动信息或计算出面部、四肢、躯干的细微变形，在运动医学、医疗康复、安全监控、身份识别、人机交互、虚拟现实、机器人设计等领域具有广泛应用前景。姿态识别的常用方法有光学式、电磁式、声学式和微机电式等几种。光学姿态识别是计算机视觉的研究内容之一，基本思想是跟踪和监视目标的特定光点，然后在视频图像序列中将人体的运动部位抽取出来。电磁姿态识别方法利用电磁感应原理，电磁发射源在空间产生规律变化的电磁场，安装在人体上的接收传感器能够收到磁场信号，当人体运动时，磁场信号发生变化，以此来进行姿态识别。声学姿态识别主要依靠检测超声波在移动设备之间的反射时间来实现，这种方法要求至少有三个已知位置的固定参考设备。以上几种识别方法，对工作环境要求较为苛刻，设计成本较高，为提高识别精度带来很大限制。

随着半导体技术的发展，微机电系统在姿态识别领域中得到越来越广泛的应用。微机电系统是指集微型机构、微型传感器、微型执行器、信号处理和控制电路、接口、通信、电源等于一体的微型器件或系统，典型器件是陀螺仪、加速度计和电子罗盘，主要依靠惯性测量单元进行姿态识别。采用微机电系统进行姿态识别的主要难点在于需通过算法设计，克服传感器输出的累积误差，以达到较高的识别精度。

1)人体姿态识别节点

本发明采用的人体姿态识别节点(以下简称为节点)由传感器模块、无线通信模块、处理器模块和电源模块四个部分组成。传感器模块集成了陀螺仪，型号为MPU3050；集成了加速度计和电子罗盘，型号为LSM303DLH。这3种传感器分别负责采集角速度、加速度和磁场强度，并将这些传感器数据从物理量转换为电信号。无线通信模块采用nRF24L01无线收发器，负责在2.4GHz～2.5GHz频段内与基站之间传输信息。处理器模块采用STM32F103嵌入式微处理器，负责控制传感器信号采集和无线收发功能。电源采用额定电压为3.7V的锂离子电池，负责为节点提供所需能量，节点提供锂离子电池接口。

节点工作时，将信号通过无线方式传输至基站，基站由处理器模块、无线通信模块和串行接口模块组成，负责接收节点发送的数据，并将接收到的数据通过串行接口传输至上位机。

2)四元数

四元数是一种超复数，由一个标量和一个矢量组成，定义如式(1)所示：

其中a、b、c、d、n₀为标量实数，i、j、k为三个虚部单位，为矢量。四元数也可表示为向量形式，即Q＝(a b c d)^T。如果Q满足a²+b²+c²+d²＝1，则将Q称作标准化四元数。

两个四元数可以进行乘法运算。设Q₁＝a+bi+cj+dk，Q₂＝e+fi+gj+hk，则按照复数运算法则计算，有i·i＝-1,i·j＝k,j·i＝-k......，因此有：

上述乘法也可表示为矩阵形式，即：

标准化四元数可用于描述向量的旋转，设为某向量，Q为标准化四元数，定义如式(1)，Q^*为Q的共轭，定义如式(4)所示：

则可用Q^*和Q将向量旋转至向量若为主动旋转，即向量逆时针围绕旋转轴旋转，则该旋转可通过式(5)来描述：

式(5)也可通过矩阵相乘来表示，如式(6)所示

其中矩阵C称为方向余弦矩阵，是由两组不同的标准正交基的基底向量之间的方向余弦所形成的矩阵，可用于向量在不同坐标系间的坐标转换。矩阵C定义如下：

若为被动旋转，即对向量所在坐标系本身进行的逆时针旋转，则可用式(8)来描述：

标准化四元数可用于描述刚体转动的角度，此时标准化四元数即为反应刚体转动方式的姿态四元数。式(9)为姿态四元数对时间的导数与转动角速度之间的关系，即四元数运动学方程：

其中，为四元数对时间的导数，p为载体沿空间三维坐标轴转动角速度构造的四元数，其实部为零，即：

p＝0+ω_xi+ω_yj+ω_zk (10)

为求解包含姿态信息的四元数，需对进行积分，然后将其化为姿态四元数，这样便可得到描述从一种姿态转动至另一种姿态的等价姿态四元数。

设x_k＝a_k+b_ki+c_kj+d_kk为姿态四元数，其中k为非负整数，表示状态序号。x_k用于描述系统在k时刻的位置状态，初始状态为x₀，其值设为1。p为代表系统当前转动角速度的四元数，复数形式定义如式(10)所示。假设从k-1时刻到k时刻经历的时间为Δt，期间刚体做匀速转动，那么根据式(9)所示四元数运动学方程，将其离散化，可以列出式(11)：

其中为姿态四元数对时间的导数，定义如式(9)所示，即：

将(12)代入(11)得：

x_k＝x_k-1+0.5·x_k-1·p·Δt (13)

式(13)即为状态向量的递推公式，如果已知x₀和p，则系统全部姿态四元数可求。求得系统姿态四元数之后，利用四元数转欧拉角的公式(14)，则可求得刚体在三维坐标系中与三个坐标轴的夹角，即得到刚体的当前位置。

其中，α_k是k时刻刚体与x轴夹角，β_k是k时刻刚体与y轴夹角，γ_k是k时刻刚体与z轴夹角。

3)卡尔曼滤波

卡尔曼滤波是高效率的自回归滤波器，能够估算大范围数据处理中的可变因素，可用于估算动态系统的状态。对于多维线性系统，式(15)为系统的状态方程，式(16)为输出方程：

x_k+1＝Ax_k+Bu_k+w_k (15)

z_k＝Hx_k+v_k (16)

其中A、B、H为系数矩阵，A称作状态转移矩阵，H称作观测矩阵，x_k为系统当前的状态，x_k+1为系统下一时刻的状态，u_k为系统当前的输入，z_k为系统当前的输出，w_k为当前的过程噪声，v_k为当前的测量噪声。式中的下标k表示当前时刻，k+1表示下一时刻。

假设过程噪声w_k的平均值为零，测量噪声v_k的平均值也为零，并且w_k和v_k相互独立。过程噪声的协方差S和测量噪声的协方差R可以分别描述为式(17)和式(18)：

定义系统当前时刻的最优状态估计值为状态期望值为则系统先验误差e'_k可表示如式(19)所示：

系统后验误差e_k可表示如式(20)所示：

先验误差的协方差如式(21)所示，即为误差协方差的期望值。

后验误差的协方差如式(22)所示：

根据卡尔曼滤波的基本原理，可由当前状态的最优估计值推测下一时刻状态的期望值，然后将系统测量误差映射到状态域，对状态的期望值进行修正，从而得到下一时刻的最优估计值。按照如上方式系统不断迭代，从而得到每一个状态的最优估计值。针对线性系统(15)和(16)的卡尔曼滤波方法由下面5个经典公式表示：

P_k'＝AP_k-1A^T+S (24)

K_k＝P_k'H^T(HP_k'H^T+R)^-1 (25)

P_k＝P_k'-K_kHP_k' (27)

式(23)表示当前时刻的状态期望值可由前一时刻状态及当前输入求得；式(24)表明了当前时刻先验误差的协方差的期望值可由前一时刻的后验误差协方差求得；式(25)表明了卡尔曼增益K_k的求解方法；求得卡尔曼增益之后，可通过式(26)，由当前时刻状态期望值和当前时刻观测值计算得到当前时刻状态最优估计值；为了使迭代能够持续不断进行，还需要通过式(27)，利用当前时刻先验误差协方差计算求得后验误差协方差，将其代入式(24)，即可求得下一时刻的先验误差协方差，从而可以开始下一轮迭代。

卡尔曼滤波的5个经典公式中，式(23)和(24)可称为时间迭代公式，用于预测状态值及误差协方差；式(25)～(27)为测量迭代公式，通过观测值修正期望值，从而获取经过改进的期望值，即最优估计值。式(26)说明了状态期望值与输出观测值在最优状态估计值构成中的比重，卡尔曼增益越大，输出观测值的比重越大，卡尔曼增益越小，最优状态估计值则越接近期望值。

自20世纪60年代以来，很多物理过程开始采用非线性模型进行描述，卡尔曼滤波适用于处理线性系统在高斯噪声下的状态估计问题，对于非线性系统则不再适用。但是，如将非线性系统在其最优估计值处进行泰勒级数展开，只取其一阶线性部分作为整体的近似表示，从而可将非线性系统转化为近似线性系统。用于处理非线性系统的卡尔曼滤波方法称作扩展卡尔曼滤波，可用于处理非线性问题，得到系统的次优状态值。

对于某非线性系统，其状态方程如(28)所示，输出方程如(29)所示：

x_k+1＝f(x_k,u_k+1)+w_k (28)

z_k＝h(x_k)+v_k (29)

其中f(·)是一个非线性函数，可通过当前时刻状态值和下一时刻控制量计算下一时刻状态值；h(·)也是一个非线性函数，通过当前状态计算系统输出值。x_k为系统当前状态，x_k+1为系统下一时刻状态，u_k为系统当前输入，z_k为系统当前输出，w_k为当前过程噪声，v_k为当前测量噪声。式中的下标k表示当前时刻，k+1表示下一时刻。过程噪声协方差矩阵S、测量噪声的协方差R、先验误差e'_k、后验误差e_k、先验误差协方差P_k'、后验误差协方差P_k的定义分别如式(17)-(22)所示。将f(·)和h(·)分别进行泰勒级数展开，并只保留一阶分量，即求f(·)和h(·)的雅克比矩阵，得到扩展卡尔曼滤波的5个公式表示如下：

P_k'＝ΦP_k-1Φ^T+S (31)

K_k＝P_k'N^T(NP_k'N^T+R)^-1 (32)

P_k＝P_k'-K_kNP_k' (34)

其中，Φ是f(·)的雅克比矩阵，N是h(·)的雅克比矩阵。

在卡尔曼滤波过程中，一方面利用测量值不断修正期望值，另一方面如果同时也对系统模型参数或噪声统计参数进行调整，则称为自适应卡尔曼滤波。例如，如果过程噪声协方差矩阵或测量噪声协方差矩阵完全未知或不能确切得知，则可在滤波过程中周期性修改过程噪声协方差矩阵和测量噪声协方差矩阵，进而通过这两个矩阵影响卡尔曼增益的大小，从而动态改变滤波效果。

本发明基于四元数运动学方程建立系统模型，采用自适应扩展卡尔曼滤波方法进行姿态识别。利用节点的陀螺仪实时检测载体姿态，计算代表载体转动角度的姿态四元数，然后结合加速度计和电子罗盘输出值进行自适应扩展卡尔曼滤波处理，校准该姿态四元数，最终得到比较准确的人体姿态识别信息。

发明内容

本发明要解决的技术问题是基于自适应扩展卡尔曼滤波，提出一种人体姿态识别方法。

本发明的技术方案：

一种基于自适应扩展卡尔曼滤波的人体姿态识别方法，步骤如下：

(1)模型设计

模型设计主要解决人体姿态识别的模型推导问题。人体运动时肢体的角度由姿态四元数来表示，采用扩展卡尔曼滤波方法，通过四元数运动学方程，根据传感器数据和当前的姿态四元数，计算下一时刻姿态四元数的期望值；对于姿态四元数x_k＝(a_k b_k c_k d_k)^T，根据式(3)所示四元数乘法的性质，可将式(13)所示四元数运动学方程的状态向量递推公式表示为式(35)，即姿态节点转动的状态方程：

其中，Δt为从k-1时刻到k时刻经历的时间，ω_x、ω_y、ω_z为空间三个轴向的转动角速度，陀螺仪采集获得；由于状态方程本身是一个线性方程，状态转移矩阵为Φ，如式(36)所示，因此进行扩展卡尔曼滤波时不需对状态转移矩阵进行泰勒级数展开。

假定已知k-1时刻姿态四元数的后验误差协方差矩阵P_k-1，k时刻姿态四元数的先验误差协方差矩阵P_k'由式(31)求得，如式(37)所示：

采用节点加速度和磁场强度作为观测值，分别对状态期望值进行校正；由于状态域和观测域的数据类型不同，需要通过观测函数h(·)将当前姿态四元数转换为载体坐标系加速度；在初始时刻测得节点的加速度值将其代入式(8)，用当前姿态四元数变换得到载体坐标系的加速度过程如式(38)所示：

通过矩阵乘法可得观测函数h(·)中各分量的表达式，如(39)所示：

按照式(40)求函数h(·)的雅克比矩阵，即令h(·)的各分量对四元数(a,b,c,d)的各项分别求偏导，可得式(41)。

式(41)中的矩阵J即为扩展卡尔曼滤波的式(32)所示的矩阵N，将其带入式(32)，即可求得卡尔曼增益K_k。

假定节点在当前时刻只有重力加速度，在没有任何误差的理想情况下，经过转换后各个方向的加速度值等于当前传感器读出的加速度值；采集姿态节点加速度计的输出值作为系统输出的观测值z_k，将其与前述求得的卡尔曼增益K_k一同代入公式(33)，对状态期望值进行修正，得到k时刻状态的次优估计值

为了使迭代能够持续进行，需要计算下一时刻的协方差矩阵P_k，将式(41)求得的N、式(31)求得的P_k'，以及卡尔曼增益K_k代入式(34)，即可求得P_k。至此实现了一次完整迭代，完成了基于扩展卡尔曼滤波算法的模型设计过程，通过该模型可得到人体运动的姿态四元数，从而完成人体姿态识别。

(2)参数设计

参数设计主要解决人体姿态识别系统的参数初始值和参数迭代计算问题，通过时变参数设计，将扩展卡尔曼滤波方法演变为自适应扩展卡尔曼滤波方法。参数设计部分要确定的参数分别为过程噪声协方差矩阵S、测量噪声协方差矩阵R、状态初始值x₀，以及误差协方差矩阵初始值P₀。

①过程噪声协方差矩阵

当前过程噪声w_k由陀螺仪的非随机噪声和随机噪声两部分构成。其中，非随机噪声主要由传感器品质、安装误差和刻度系数误差引起；随机噪声主要由逐次启动漂移、慢变漂移和快变漂移产生。在实际工作过程中，通过静态标定可以减弱非随机噪声和逐次启动漂移的影响，因此可近似认为w_k仅包含慢变漂移和快变漂移产生的噪声，并将其近似认为是高斯白噪声。

建立过程噪声协方差矩阵的数学模型，将系统状态方程表示为式(42)：

其中，误差主要由等号右面第二项决定，即如式(43)所示：

其中，代表x轴角速度的误差，代表y轴角速度的误差，代表z轴角速度的误差，Ψ_k-1为前一时刻状态分量组成的4×4方阵；所以，将过程噪声的协方差矩阵S表示如式(44)所示：

其中，ω_S为式中的协方差因子，如式(45)所示：

其中，为(45)中等式最右边方阵中右下角的3×3方阵。由于Ψ_k-1由前一时刻状态的各分量组成，因此在扩展卡尔曼公式递推过程中为已知，所以过程噪声协方差矩阵最终归结为计算即三轴角速度误差的协方差矩阵。同时，也由于Ψ_k-1由前一时刻状态的各分量组成，因此Ψ_k-1的值时时变化，因此本系统的处理方法实质为自适应扩展卡尔曼滤波。

可按照下述方法估计：将节点置于水平桌面上保持静止不动，采集1000次陀螺仪的三轴角速度值，得到1000个三维向量，计算协方差矩阵，即可得到如式(46)所示：

将式(46)代入式(45)计算得到ω_S，如式(47)所示。

将式(47)代入式(44)，结合前一时刻状态值，即可求出过程噪声协方差矩阵S。

②测量噪声协方差矩阵

v_k为当前测量噪声。在惯性导航系统中，测量噪声可近似认为是由一阶马尔科夫过程产生，近似等效为高斯白噪声。

测量噪声协方差矩阵按照如下方式来估计。首先按照六位置法对加速度计进行标定，然后将节点静止放置于水平桌面，然后采集1000次加速度计的三轴输出，得到1000个三维向量，计算协方差矩阵，得到式(48)：

从式(48)可知，非对角元值比对角元小1个数量级，考虑到测量精度及传感器字长限制，可近似认为加速度计三个轴向的测量噪声相互独立，因此可将所有非对角元取值为0，即得到加速度计的测量噪声协方差矩阵如式(49)所示：

同理，将节点静止放置于水平桌面，然后采集1000次电子罗盘的三轴输出，得到1000个三维向量，计算协方差矩阵，得到式(50)：

基于同样原因，可近似认为电子罗盘三个轴向的测量噪声相互独立，因此可将所有非对角元取值为0，即得到电子罗盘的测量噪声协方差矩阵如式(51)所示：

③滤波初始条件估算

假定系统一致完全随机可控且一致完全随机可观，则卡尔曼滤波器具有一致渐近稳定性，随着滤波步数的增加，状态初值x₀和误差协方差矩阵初值P₀对状态值x_k和P_k的影响将逐渐消失，使估计值趋向无偏，因此x₀和P₀的选择范围较为宽松。实际工作时，选择初始状态x₀为单位四元数(1,0,0,0)，选择P₀初始值为单位阵。

本发明的有益效果在于，提出了一种新的人体姿态识别方法。该方法采集人体运动的角速度、加速度和人体周边磁场强度数据，通过自适应扩展卡尔曼滤波进行姿态解算，计算得到代表人体肢体运动角度的四元数，从而实现人体运动姿态识别。本方法可用于体育训练、医疗保健、游戏设计等领域。

附图说明

图1是本发明方法的流程图。

图2是本发明方法的x轴测量值补偿前后误差对比。

图3是本发明方法的y轴测量值补偿前后误差对比。

图4是本发明方法的z轴测量值补偿前后误差对比。

图5是本发明方法的节点仿真模型随动验证结果，图分别为节点转动到各个角度时，节点姿态与计算机中节点仿真模型姿态的对比图。

图6是本发明方法的人体仿真模型随动验证结果，图①-⑨分别为佩戴节点后的各类型腿部姿态与计算机中人体仿真模型姿态的对比图。

具体实施方式

以下结合发明内容和说明书附图详细说明本发明的具体实施方式。

(1)方法概述

本发明采用的节点上包含陀螺仪、加速度计和电子罗盘等传感器，分别用于测量节点转动的角速度、直线运动的加速度，以及节点位置的磁场强度。采用惯性传感器设计的姿态识别节点是一个典型的非线性系统，离散状态方程和输出方程如式(28)和(29)所示。系统由于受到噪声的干扰，无法精确得到状态值，而只能在一定统计意义上对状态值进行估计。扩展卡尔曼滤波适用于对非线性系统的随机信号进行处理，能够从与被提取信号有关的测量值中估计出所需信号，测量值与被估计量之间的函数关系也已知，因此扩展卡尔曼滤波适用于求解非线性随机离散时间系统的状态估计问题。同时，由于模型永远无法与真实系统完全等同，不可避免存在过程噪声。对过程噪声建模，将其分为恒定和时变两个部分，其中时变部分在每个迭代周期均重新计算，因此采用的方式实际是一种自适应扩展卡尔曼滤波方法。

由于四元数可描述刚体运动规律，能够将三维空间的向量做任意旋转。节点的加速度计用于采集载体坐标系下三轴的加速度分量，可表示为三维向量形式。因此，如果在参考坐标系下节点的加速度大小和方向不变，便可利用四元数建立起相邻两个时刻加速度向量之间的转换关系。利用上一时刻陀螺仪输出和状态方程求得当前时刻的状态期望值，利用该期望值计算当前时刻的加速度计输出，然后采集当前时刻的加速度计实际测量值，将二者进行对比，利用其残差和卡尔曼增益对当前时刻状态期望值进行校正，得到状态的估计值。节点的电子罗盘输出同样为三维向量形式，姿态校正过程与加速度计完全相同。系统经过多次迭代，使姿态四元数的期望值不断接近真实值，最终得到代表肢体运动角度的较为准确的姿态四元数。

(2)模型设计

先不考虑人体姿态识别系统中参数的时变部分，在系统参数定常时，姿态识别问题可用扩展卡尔曼滤波方法来处理。

扩展卡尔曼滤波的第一个步骤是根据系统当前状态，通过函数f(·)计算下一状态的预测值。在本问题中为通过四元数运动学方程建模，根据陀螺仪测得的角速度和当前姿态四元数，计算下一时刻姿态四元数的期望值。

对于姿态四元数x_k＝(a_k b_k c_k d_k)^T，根据式(3)所示四元数乘法的性质，可将递推公式(13)表示为下式：

整理得：

这就是姿态节点转动的状态方程，可知状态方程本身即为线性方程，因此不需进行泰勒级数展开。同时，由于姿态节点转动时没有控制量，所以式(35)中不包括控制部分。由式(35)可知系统状态转移矩阵Φ如式(36)所示：

式(36)中的转动角速度可用陀螺仪采集获得，如果x_k-1为k-1时刻状态的估计值则x_k为k时刻状态的期望值可由式(30)可得到式(53)。

假定已知k-1时刻姿态四元数的后验误差协方差矩阵P_k-1，k时刻姿态四元数的先验误差协方差矩阵P_k'可由式(31)求得，如式(37)所示。

在经过扩展卡尔曼滤波时间迭代公式后，可根据陀螺仪信号得到当前时刻姿态四元数的期望值。由于陀螺仪信号存在漂移，因而在积分过程中易产生累积误差，需通过引入新的参考量减小累积误差对姿态解算带来的影响。

假设在采样过程中，参考坐标系的加速度、磁场强度不发生改变，在校正过程当中，如果将当前时刻姿态四元数的期望值作用于参考坐标系下的加速度和磁场强度，使此二者进行被动旋转，便可求得当前时刻载体坐标系加速度和磁场强度的期望值，进而能够与当前时刻加速度和磁场强度的观测值进行比对求得残差，以此残差来校正姿态四元数。

为了借助观测值对期望值进行修正，首先应求得卡尔曼增益K_k。采用节点加速度和磁场强度作为观测值，分别对状态期望值进行校正，这两种校正方式完全相同，下面以加速度为例说明。

由于状态域和观测域的数据类型不同，需要通过观测函数h(·)将当前姿态四元数转换为载体坐标系加速度。在初始时刻测得节点的加速度值将其带入式(8)，用当前姿态四元数变换得到载体坐标系的加速度过程如式(38)所示：

通过矩阵乘法可得观测函数h(·)中各分量的表达式，如(39)所示：

按照式(40)求函数h(·)的雅克比矩阵，即令h(·)的各分量对四元数(a,b,c,d)的各项分别求偏导，可得式(41)。

式(41)中的矩阵J即为扩展卡尔曼滤波的式(32)所示的矩阵N，将其带入式(32)，可求得卡尔曼增益K_k。

式(32)中，需要对矩阵NP_k'N^T+R求逆。矩阵可逆的充要条件是其行列式不为零，或矩阵满秩，下面进行分析。

在参数选择时，选取测量噪声协方差矩阵R为正定阵。P_k'是k时刻的先验误差协方差矩阵，定义如(54)所示：

式中e'_k表示k时刻系统的期望值与真实值的差值，即先验误差。根据式(55)所示协方差矩阵的性质：

cov(Ux,Vx)＝Ucov(x,x)V^T (55)

可得式(56)：

NP_k'N^T＝Ncov(e'_k,e'_k)N^T＝cov(Ne'_k,Ne'_k) (56)

由该公式可知，NP_k'N^T是Ne'_k的协方差矩阵，因此NP_k'N^T至少为一个半正定阵，根据半正定阵的定义，可知存在任意一个非零向量W，使得式(57)成立：

WNP_k'N^TW^T≥0 (57)

又因为R为正定矩阵，可知非零向量W使得式(58)成立：

WR W^T＞0 (58)

令R的维度为m，P_k'的维度为n，已知矩阵N是m×n矩阵，所以NP_k'N^T与R同为m阶方阵。因此，将式(57)和式(58)相加，可得(59)：

因此，根据正定矩阵的定义可知，为NP_k'N^T+R正定矩阵，所以其必然可逆。

假定节点在当前时刻只有重力加速度，在没有任何误差的理想情况下，经过转换后各个方向的加速度值应该等于当前传感器读出的加速度值。采集姿态节点加速度计的输出值作为系统输出的观测值z_k，将其与前述求得的卡尔曼增益K_k一同代入公式(33)，对状态期望值进行修正，得到k时刻状态的次优估计值

为了使迭代能够持续进行，需要计算下一时刻的协方差矩阵P_k，将式(41)求得的N、式(37)求得的P_k'，以及卡尔曼增益K_k代入式(34)，即可求得P_k。至此，完成了基于扩展卡尔曼滤波算法，采用加速度计观测值对姿态四元数进行修正的迭代过程。

(3)参数设计

在扩展卡尔曼滤波的计算过程中，涉及到一系列参数初始化问题，分别为过程噪声协方差矩阵S、测量噪声协方差矩阵R、状态初始值x₀，以及误差协方差矩阵初始值P₀，下面分别进行讨论。

①过程噪声协方差矩阵

在式(28)中，w_k为当前过程噪声，该噪声主要由陀螺仪的非随机噪声和随机噪声两部分构成。其中，非随机噪声主要由传感器品质、安装误差和刻度系数误差引起，是节点的固有特征，在一定时期内基本保持不变，可采用静态标定的方法予以补偿。陀螺仪的随机噪声主要由逐次启动漂移、慢变漂移和快变漂移产生。其中，逐次启动漂移体现为一个随机常数，在陀螺仪工作过程中保持不变，因此可等效为常值偏差，也可通过标定来消除。慢变漂移是由工作环境、电气参数的随机变化引起的，可近似用一阶马尔科夫过程描述，近似视为白噪声。快变漂移表现为系统的杂乱无章的高频跳变，也可抽象为白噪声。

在系统实际工作过程中，通过静态标定可以减弱非随机噪声和逐次启动漂移的影响，因此可近似认为w_k仅包含慢变漂移和快变漂移产生的噪声，并将其近似认为是高斯白噪声。

基于以上分析，建立过程噪声协方差矩阵的数学模型。由于本系统的状态方程为线性方程，并且过程噪声主要由陀螺仪引起，根据式(35)可知，ω_x、ω_y和ω_z是误差的主要来源，因此可将式(35)转换成式(42)：

式(42)中，等号右边的各状态分量已知，而且认为是上一状态的次优估计值，则误差部分主要由等号右面第二项决定，即误差如式(43)所示：

其中，代表x轴角速度的误差，代表y轴角速度的误差，代表z轴角速度的误差，Ψ_k-1为前一时刻状态分量组成的4×4方阵。所以，将过程噪声的协方差矩阵S表示如式(44)所示：

其中，ω_S为式中的协方差因子，如式(45)所示：

其中，为(45)中等式最右边方阵中右下角的3×3方阵。由于Ψ_k-1由前一时刻状态的各分量组成，因此在扩展卡尔曼公式递推过程中为已知，所以过程噪声协方差矩阵最终归结为计算即三轴角速度误差的协方差矩阵。同时，也由于Ψ_k-1由前一时刻状态的各分量组成，因此Ψ_k-1的值时时变化，因此本系统的处理方法实质为自适应扩展卡尔曼滤波。可按照下述方法估计：将节点置于水平桌面上保持静止不动，采集1000次陀螺仪的三轴角速度值，得到1000个三维向量，计算协方差矩阵，即可得到如式(46)所示：

将式(46)代入式(45)计算得到ω_S，如式(47)所示。

将式(47)代入式(44)，结合前一时刻状态值，即可求出过程噪声协方差矩阵S。

②测量噪声协方差矩阵

在式(29)中，v_k为当前测量噪声。在惯性导航系统中，测量噪声可近似认为是由一阶马尔科夫过程产生，近似等效为高斯白噪声。

测量噪声协方差矩阵按照如下方式来估计。首先按照六位置法对加速度计进行标定，然后将节点静止放置于水平桌面，然后采集1000次加速度计的三轴输出，得到1000个三维向量，计算协方差矩阵，得到式(48)：

从式(48)可知，非对角元值比对角元小1个数量级，考虑到测量精度及传感器字长限制，可近似认为加速度计三个轴向的测量噪声相互独立，因此可将所有非对角元取值为0，即得到加速度计的测量噪声协方差矩阵如式(49)所示：

同理，将节点静止放置于水平桌面，然后采集1000次电子罗盘的三轴输出，得到1000个三维向量，计算协方差矩阵，得到式(50)：

基于同样原因，可近似认为电子罗盘三个轴向的测量噪声相互独立，因此可将所有非对角元取值为0，即得到电子罗盘的测量噪声协方差矩阵如式(51)所示：

因此，本系统加速度计和电子罗盘的测量噪声协方差矩阵分别采用(49)和(51)所示矩阵。

从式(49)和式(51)可知，加速度计和电子罗盘的测量噪声协方差矩阵均为正定阵，因此前文中提及的NP_k'N^T+R可逆的前提条件存在。

③滤波初始条件估算

假定系统一致完全随机可控且一致完全随机可观，则卡尔曼滤波器具有一致渐近稳定性，随着滤波步数的增加，状态初值x₀和误差协方差矩阵初值P₀对状态值x_k和P_k的影响将逐渐消失，使估计值趋向无偏，因此x₀和P₀的选择范围较为宽松。实际工作时，只能对x₀和P₀初始值进行估计选取，较为准确的估计值能够加快滤波收敛速度，反之则系统收敛速度变慢。本系统中，选择初始状态x₀为单位四元数(1,0,0,0)，选择P₀初始值为单位阵，实验表明在该初始值设定下，系统状态能够较快收敛。

(4)实验及分析

采用自适应扩展卡尔曼滤波方法对姿态四元数角度进行补偿，之后与不进行补偿的数据进行对比，了解补偿效果。将姿态四元数传输至上位机，利用OpenGL设计节点仿真模型，实现对节点转动情况的复现，验证节点采集姿态信息的准确性。利用OpenGL设计人体仿真模型，将节点固定在腿部，抬起腿部，观察人体仿真模型的工作状况，验证姿态识别准确性。

①传感器输出补偿

采用节点测量不同角度下读数，判断采用自适应扩展卡尔曼滤波方法进行补偿的实际效果。将节点固定在转台之上转动，近似认为转台示数为真实值，传感器读数为测量值。从0度到1080度，每转动90度读取两次姿态四元数值，其中一次是未采用自适应扩展卡尔曼滤波方法求得的姿态四元数，另一次则经过了补偿，合计读取26组姿态四元数。然后将二者分别转换为x轴的角度。再将转台直立于水平面，按照上述步骤分别测量y轴和z轴的角度。合计读取补偿前后的姿态四元数各39个，按照四元数转欧拉角的式(14)求得三轴对应角度，并将每一个角度值与角度真实值相减求得误差。补偿前后的角度误差如表1所示。

表1三轴输出角度误差补偿前后对比

由表1可知，当载体转动角度较小时，真实值和补偿前后的测量值相差不大。随着转角不断增加，未加补偿的测量值与真实值之间的误差迅速增加，总体呈现正相关趋势；而补偿后的测量值始终较为接近真实值，在0～1080度测量区间内，误差始终在正负1度范围内。x轴、y轴和z轴在补偿前后的误差曲线分别如图2、图3和图4所示，横坐标为转动角度，纵坐标为误差。可见对于三个坐标轴来说，经过补偿后的曲线相较补偿之前更接近真实值，大幅提高了系统测量准确性。

②节点仿真模型随动验证

将节点计算得到的姿态四元数经基站传递到上位机，采用OpenGL设计节点仿真模型来重现节点当前转动角度，并利用四元数驱动该节点仿真模型转动，节点仿真模型即图5中显示在计算机屏幕上的立方体。以不超过每秒90度的角速度随机转动节点，观察节点仿真模型随动转动情况，如图5所示。

观察实验现象，当节点随机转动时，显示在计算机屏幕上的节点仿真模型能够跟随转动，说明节点能够正确采集自身姿态，并将姿态信息传递到上位机，驱动节点仿真模型同步转动。由于系统采样及信息传输不可避免具有一定滞后，节点仿真模型的运动相比节点运动来说也有一定滞后，但是当节点以不超过每秒90度左右的角速度转动时，肉眼基本观察不到滞后。进一步加快节点转动角速度时，可感觉到节点仿真模型运动滞后，但是一旦降低节点速度，节点仿真模型当前转动角度会迅速向节点实际转动角度趋近，表明自适应扩展卡尔曼滤波算法的实际收敛速度很快。

③人体仿真模型随动验证

设计基于OpenGL的人体仿真模型来模拟测试者腿部转动。将姿态节点固定在测试者腿部，当腿部转动时，姿态节点采集姿态四元数，然后通过wifi将四元数发送到基站，进而传输到运行于上位机的人体仿真模型，驱动人体仿真模型的腿部跟随测试者腿部转动。实验情况如图6所示。

当腿部转动角速度低于每秒90度时，运行于上位机的人体仿真模型腿部能够同步转动，当腿部转动角速度继续增长时，人体仿真模型腿部转动开始出现滞后，但是一旦腿部运动角速度下降，人体仿真模型腿部的角度迅速趋近于测试者腿部的角度，说明自适应扩展卡尔曼滤波算法能够快速收敛。

Claims (1)

1.一种基于自适应扩展卡尔曼滤波的人体姿态识别方法，其特征在于，

本方法基于扩展卡尔曼滤波的如下5个公式设计：

${\hat{x}}_k^{\′}=f({\hat{x}}_{k-1},u_k)+w_k---\left(30\right)$

P′_k＝ΦP_k-1Φ^T+S (31)

K_k＝P′_kN^T(NP_k'N^T+R)^-1 (32)

${\hat{x}}_k={\hat{x}}_k^{\′}+K_k(z_k-h({\hat{x}}_k^{\′}\left)\right)---\left(33\right)$

P_k＝P′_k-K_kNP′_k (34)

其中，k表示当前时刻，k-1表示前一时刻；通过式(30)由前一时刻状态最优估计值和当前时刻系统输入计算当前时刻状态期望值，其中，为当前时刻状态期望值，x_k-1为前一时刻状态最优估计值，u_k为系统当前时刻的输入，w_k为当前时刻的过程噪声，f(·)是一个非线性函数；式(31)为系统误差迭代公式，其中，P_k'为先验误差协方差，P_k-1为后验误差协方差，Φ是f(·)的雅克比矩阵，S为过程噪声协方差矩阵；式(32)用于计算卡尔曼增益，其中，K_k为卡尔曼增益，N为式(33)中h(·)的雅克比矩阵，R为测量噪声的协方差；式(33)为状态迭代公式，当前状态的最优估计值经由当前状态期望值和残差求得，其中，为系统当前时刻的最优状态估计值，z_k为系统当前输出，h(·)也是一个非线性函数；式(34)为误差协方差迭代公式；

本方法基于节点工作，节点集成了陀螺仪、加速度计和电子罗盘，分别负责采集角速度、加速度和磁场强度传感器数据，并将传感器数据从物理量转换为电信号；

该方法包括模型设计和参数设计两个部分；

(1)模型设计用于解决人体姿态识别的模型推导问题；人体运动时肢体的角度由姿态四元数来表示，采用扩展卡尔曼滤波方法，通过四元数运动学方程，根据传感器数据和当前姿态四元数，计算下一时刻姿态四元数的期望值；对于姿态四元数x_k＝(a_k b_k c_k d_k)^T，根据四元数乘法的性质，将四元数运动学方程的状态向量递推公式表示为式(35)，即节点转动的系统状态方程：

$x_k=\left[\begin{array}{c}{a}_k\\ b_k\\ c_k\\ d_k\end{array}\right]=\left[\begin{array}{cccc}1& -0.5\·{\mathrm{\Δt\ω}}_x& -0.5\·{\mathrm{\Δt\ω}}_y& -0.5\·{\mathrm{\Δt\ω}}_z\\ 0.5\·{\mathrm{\Δt\ω}}_x& 1& 0.5\·{\mathrm{\Δt\ω}}_z& -0.5\·{\mathrm{\Δt\ω}}_y\\ 0.5\·{\mathrm{\Δt\ω}}_y& -0.5\·{\mathrm{\Δt\ω}}_z& 1& 0.5\·{\mathrm{\Δt\ω}}_x\\ 0.5\·{\mathrm{\Δt\ω}}_z& 0.5\·{\mathrm{\Δt\ω}}_y& -0.5\·{\mathrm{\Δt\ω}}_x& 1\end{array}\right]\left[\begin{array}{c}{a}_{k-1}\\ b_{k-1}\\ c_{k-1}\\ d_{k-1}\end{array}\right]={\mathrm{\Φx}}_{k-1}---\left(35\right)$

其中，Δt为从k-1时刻到k时刻经历的时间，ω_x、ω_y、ω_z为空间三个轴向的转动角速度，由陀螺仪采集获得；由于状态方程本身是一个线性方程，转移矩阵为Φ，如式(36)所示，因此进行扩展卡尔曼滤波时不需对状态转移矩阵进行泰勒级数展开；

$\mathrm{\Φ}=\left[\begin{array}{cccc}1& -0.5\·{\mathrm{\Δt\ω}}_x& -0.5\·{\mathrm{\Δt\ω}}_y& -0.5\·{\mathrm{\Δt\ω}}_z\\ 0.5\·{\mathrm{\Δt\ω}}_x& 1& 0.5\·{\mathrm{\Δt\ω}}_z& -0.5\·{\mathrm{\Δt\ω}}_y\\ 0.5\·{\mathrm{\Δt\ω}}_y& -0.5\·{\mathrm{\Δt\ω}}_z& 1& 0.5\·{\mathrm{\Δt\ω}}_x\\ 0.5\·{\mathrm{\Δt\ω}}_z& 0.5\·{\mathrm{\Δt\ω}}_y& -0.5\·{\mathrm{\Δt\ω}}_x& 1\end{array}\right]---\left(36\right)$

假定已知k-1时刻姿态四元数的后验误差协方差矩阵P_k-1，k时刻姿态四元数的先验误差协方差矩阵P_k'由式(31)求得，如式(37)所示：

$\begin{array}{c}{P}_k^{\′}=\\ \left[\begin{array}{cccc}1& -0.5{\mathrm{\Δt\ω}}_x& -0.5{\mathrm{\Δt\ω}}_y& -0.5{\mathrm{\Δt\ω}}_z\\ 0.5{\mathrm{\Δt\ω}}_x& 1& 0.5{\mathrm{\Δt\ω}}_z& -0.5{\mathrm{\Δt\ω}}_y\\ 0.5{\mathrm{\Δt\ω}}_y& -0.5{\mathrm{\Δt\ω}}_z& 1& 0.5{\mathrm{\Δt\ω}}_x\\ 0.5{\mathrm{\Δt\ω}}_z& 0.5{\mathrm{\Δt\ω}}_y& -0.5{\mathrm{\Δt\ω}}_x& 1\end{array}\right]P_{k-1}\left[\begin{array}{cccc}1& -0.5{\mathrm{\Δt\ω}}_x& -0.5{\mathrm{\Δt\ω}}_y& -0.5{\mathrm{\Δt\ω}}_z\\ -0.5{\mathrm{\Δt\ω}}_x& 1& -0.5{\mathrm{\Δt\ω}}_z& -0.5{\mathrm{\Δt\ω}}_y\\ -0.5{\mathrm{\Δt\ω}}_y& -0.5{\mathrm{\Δt\ω}}_z& 1& -0.5{\mathrm{\Δt\ω}}_x\\ -0.5{\mathrm{\Δt\ω}}_z& -0.5{\mathrm{\Δt\ω}}_y& -0.5{\mathrm{\Δt\ω}}_x& 1\end{array}\right]+S\end{array}---\left(37\right)$

采用节点加速度和磁场强度作为观测值，分别对状态期望值进行校正；由于状态域和观测域的数据类型不同，通过观测函数h(·)将当前姿态四元数转换为载体坐标系加速度；设初始时刻测得节点的加速度值对向量所在坐标系本身进行逆时针旋转，用当前姿态四元数(a,b,c,d)变换得到载体坐标系的加速度如式(38)所示：

${\stackrel{\→}{a}}^{\′}=C^T\stackrel{\→}{a}=\left[\begin{array}{ccc}{a}^2+b^2-c^2-d^2& 2(bc+ad)& 2(bd-ac)\\ 2(bc-ad)& a^2-b^2+c^2-d^2& 2(cd+ab)\\ 2(bd+ac)& 2(cd-ab)& a^2-b^2-c^2+d^2\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]---\left(38\right)$

通过矩阵乘法得出观测函数h(·)中各分量的表达式，如式(39)所示：

$\left\{\begin{array}{c}{a}_x^{\′}=(a^2+b^2-c^2-d^2)a_x+2(bc+ad)a_y+2(bd-ac)a_z\\ a_y^{\′}=2(bc-ac)a_x+(a^2-b^2+c^2-d^2)a_y+2(cd+ab)a_z\\ a_z^{\′}=2(bd+ac)a_x+2(cd-ab)a_y+(a^2-b^2-c^2+d^2)a_z\end{array}\right.---\left(39\right)$

按照式(40)求函数h(·)的雅克比矩阵，即令h(·)的各分量对四元数(a,b,c,d)的各项分别求偏导，得式(41)：

$J=\left[\begin{array}{cccc}\frac{\∂{\stackrel{\→}{a}}_x^{\′}}{\∂a}& \frac{\∂{\stackrel{\→}{a}}_x^{\′}}{\∂b}& \frac{\∂{\stackrel{\→}{a}}_x^{\′}}{\∂c}& \frac{\∂{\stackrel{\→}{a}}_x^{\′}}{\∂d}\\ \frac{\∂{\stackrel{\→}{a}}_y^{\′}}{\∂a}& \frac{\∂{\stackrel{\→}{a}}_y^{\′}}{\∂b}& \frac{\∂{\stackrel{\→}{a}}_y^{\′}}{\∂c}& \frac{\∂{\stackrel{\→}{a}}_y^{\′}}{\∂d}\\ \frac{\∂{\stackrel{\→}{a}}_z^{\′}}{\∂a}& \frac{\∂{\stackrel{\→}{a}}_z^{\′}}{\∂b}& \frac{\∂{\stackrel{\→}{a}}_z^{\′}}{\∂c}& \frac{\∂{\stackrel{\→}{a}}_z^{\′}}{\∂d}\end{array}\right]---\left(40\right)$

$J=2\×\left[\begin{array}{cccc}a\·a_x-c\·a_z+d\·a_y& b\·a_x+c\·a_y+d\·a_z& b\·b_y-a\·a_z-c\·a_x& a\·a_y+b\·a_z-d\·a_x\\ a\·a_y+b\·a_z-d\·a_x& a\·a_z-b\·a_y+c\·a_x& b\·a_x+c\·a_y+d\·a_z& c\·a_z-a\·a_x-d\·a_y\\ a\·a_x-b\·a_y+c\·a_x& d\·a_x-b\·a_z-a\·a_y& a\·a_x-c\·a_z+d\·a_y& b\·a_x+c\·a_y+d\·a_z\end{array}\right]---\left(41\right)$

式(41)中的矩阵J即为扩展卡尔曼滤波的式(32)所示的矩阵N，将其带入式(32)，求得卡尔曼增益K_k；

假定节点在当前时刻只有重力加速度，在没有任何误差的理想情况下，经过转换后各个方向的加速度值等于当前传感器读出的加速度值；采集节点加速度计的输出值作为系统输出的观测值z_k，将其与前述求得的卡尔曼增益K_k一同代入公式(33)，对状态期望值进行修正，得到k时刻状态的次优估计值为了使迭代持续进行，计算下一时刻的协方差矩阵P_k，将式(41)求得的N、式(37)求得的P_k'以及卡尔曼增益K_k代入式(34)，即求得P_k，至此实现了一次完整迭代，完成了基于扩展卡尔曼滤波算法的模型设计过程，通过该模型得到人体运动的姿态四元数，从而完成人体姿态识别；

(2)参数设计用于解决人体姿态识别的参数初始值和参数迭代计算问题，由于参数具有时变部分，本扩展卡尔曼滤波方法演变为自适应扩展卡尔曼滤波方法；参数设计部分要确定的参数分别为过程噪声协方差矩阵S、测量噪声协方差矩阵R、状态初始值x₀，以及误差协方差矩阵初始值P₀；

①过程噪声协方差矩阵

当前过程噪声w_k由陀螺仪的非随机噪声和随机噪声两部分构成；其中，非随机噪声由传感器品质、安装误差和刻度系数误差引起；随机噪声由逐次启动漂移、慢变漂移和快变漂移产生；在实际工作过程中，通过静态标定减弱非随机噪声和逐次启动漂移的影响，因此认为w_k仅包含慢变漂移和快变漂移产生的噪声，并将其认为是高斯白噪声；

建立过程噪声协方差矩阵的数学模型，将式(35)所示系统状态方程表示为式(42)：

$x_k=I\·\left[\begin{array}{c}{a}_{k-1}\\ b_{k-1}\\ c_{k-1}\\ d_{k-1}\end{array}\right]+0.5\·\mathrm{\Δ}t\·\left[\begin{array}{cccc}{a}_{k-1}& -b_{k-1}& -c_{k-1}& -d_{k-1}\\ b_{k-1}& a_{k-1}& -d_{k-1}& c_{k-1}\\ c_{k-1}& -d_{k-1}& a_{k-1}& -b_{k-1}\\ d_{k-1}& -c_{k-1}& b_{k-1}& a_{k-1}\end{array}\right]\·\left[\begin{array}{c}0\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\end{array}\right]---\left(42\right)$

其中，误差由等号右面第二项决定，即如式(43)所示：

${\widetilde{\mathrm{\ω}}}_k=0.5\·\mathrm{\Δ}t\·\left[\begin{array}{cccc}{a}_{k-1}& -b_{k-1}& -c_{k-1}& -d_{k-1}\\ b_{k-1}& a_{k-1}& -d_{k-1}& c_{k-1}\\ c_{k-1}& -d_{k-1}& a_{k-1}& -b_{k-1}\\ d_{k-1}& -c_{k-1}& b_{k-1}& a_{k-1}\end{array}\right]\·\left[\begin{array}{c}0\\ {\widetilde{\mathrm{\ω}}}_x\\ {\widetilde{\mathrm{\ω}}}_y\\ {\widetilde{\mathrm{\ω}}}_z\end{array}\right]=0.5\·\mathrm{\Δ}t\·{\mathrm{\Ψ}}_{k-1}\·\left[\begin{array}{c}0\\ {\widetilde{\mathrm{\ω}}}_x\\ {\widetilde{\mathrm{\ω}}}_y\\ {\widetilde{\mathrm{\ω}}}_z\end{array}\right]---\left(43\right)$

其中，代表x轴角速度的误差，代表y轴角速度的误差，代表z轴角速度的误差，Ψ_k-1为前一时刻状态分量组成的4×4方阵；所以，将过程噪声的协方差矩阵S表示如式(44)所示：

$S=\mathrm{cov}({\widetilde{\mathrm{\ω}}}_k,{\widetilde{\mathrm{\ω}}}_k)={\left(0.5\mathrm{\Δ}t\right)}^2\·{\mathrm{\Ψ}}_{k-1}\·\mathrm{cov}(\left[\begin{array}{c}0\\ {\widetilde{\mathrm{\ω}}}_x\\ {\widetilde{\mathrm{\ω}}}_y\\ {\widetilde{\mathrm{\ω}}}_z\end{array}\right],\left[\begin{array}{c}0\\ {\widetilde{\mathrm{\ω}}}_x\\ {\widetilde{\mathrm{\ω}}}_y\\ {\widetilde{\mathrm{\ω}}}_z\end{array}\right])\·{\mathrm{\Ψ}}_{k-1}^T={\left(0.5\mathrm{\Δ}t\right)}^2\·{\mathrm{\Ψ}}_{k-1}\·{\mathrm{\ω}}_S\·{\mathrm{\Ψ}}_{k-1}^T---\left(44\right)$

其中，ω_S为式中的协方差因子，如式(45)所示：

${\mathrm{\ω}}_S=\cos (\left[\begin{array}{c}0\\ {\widetilde{\mathrm{\ω}}}_x\\ {\widetilde{\mathrm{\ω}}}_y\\ {\widetilde{\mathrm{\ω}}}_z\end{array}\right],\left[\begin{array}{c}0\\ {\widetilde{\mathrm{\ω}}}_x\\ {\widetilde{\mathrm{\ω}}}_y\\ {\widetilde{\mathrm{\ω}}}_z\end{array}\right])=\left[\begin{array}{cc}0& 0\\ 0& \mathrm{cov}(\widetilde{\mathrm{\ω}},\widetilde{\mathrm{\ω}})\end{array}\right]---\left(45\right)$

其中，为(45)中等式最右边方阵中右下角的3×3方阵；由于Ψ_k-1由前一时刻状态的各分量组成，因此在扩展卡尔曼公式递推过程中为已知，所以过程噪声协方差矩阵最终归结为计算即三轴角速度误差的协方差矩阵；同时，也由于Ψ_k-1由前一时刻状态的各分量组成，因此Ψ_k-1的值时时变化，因此本系统的处理方法实质为自适应扩展卡尔曼滤波；

按照下述方法估计：将节点置于水平桌面上保持静止不动，采集1000次陀螺仪的三轴角速度值，得到1000个三维向量，计算协方差矩阵，即得到如式(46)所示：

$\mathrm{cov}(\widetilde{\mathrm{\ω}},\widetilde{\mathrm{\ω}})=\left[\begin{array}{ccc}0.0198& 0.0022& -0.0019\\ 0.0022& 0.0233& -0.0035\\ -0.0019& -0.0035& 0.0207\end{array}\right]---\left(46\right)$

将式(46)代入式(45)计算得到ω_S，如式(47)所示；

${\mathrm{\ω}}_S=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0.0198& 0.0022& -0.0019\\ 0& 0.0022& 0.0233& -0.0035\\ 0& -0.0019& -0.0035& 0.0207\end{array}\right]---\left(47\right)$

将式(47)代入式(44)，结合前一时刻状态值，即求出过程噪声协方差矩阵S；

②测量噪声协方差矩阵

v_k为当前测量噪声；在惯性导航系统中，测量噪声认为是由一阶马尔科夫过程产生，近似等效为高斯白噪声；

测量噪声协方差矩阵按照如下方式来估计；首先按照六位置法对加速度计进行标定，然后将节点静止放置于水平桌面，然后采集1000次加速度计的三轴输出，得到1000个三维向量，计算协方差矩阵，得到式(48)：

$R_{ACC}=\left[\begin{array}{ccc}0.0588& 0.0047& 0.0025\\ 0.0047& 0.0367& 0.0020\\ 0.0025& 0.0020& 0.0362\end{array}\right]\×{10}^{-4}---\left(48\right)$

从式(48)知，非对角元值比对角元小1个数量级，考虑到测量精度及传感器字长限制，认为加速度计三个轴向的测量噪声相互独立，因此将所有非对角元取值为0，即得到加速度计的测量噪声协方差矩阵如式(49)所示：

$R_{ACC}=\left[\begin{array}{ccc}0.0588& 0& 0\\ 0& 0.0367& 0\\ 0& 0& 0.0362\end{array}\right]\×{10}^{-4}---\left(49\right)$

同理，将节点静止放置于水平桌面，然后采集1000次电子罗盘的三轴输出，得到1000个三维向量，计算协方差矩阵，得到式(50)：

$R_{MAG}=\left[\begin{array}{ccc}0.8030& -0.0252& 0.0411\\ -0.0252& 0.6868& -0.0758\\ 0.0411& -0.0758& 0.7742\end{array}\right]\×{10}^{-4}---\left(50\right)$

基于同样原因，认为电子罗盘三个轴向的测量噪声相互独立，因此将所有非对角元取值为0，即得到电子罗盘的测量噪声协方差矩阵如式(51)所示：

$R_{MAG}=\left[\begin{array}{ccc}0.8030& 0& 0\\ 0& 0.6868& 0\\ 0& 0& 0.7742\end{array}\right]\×{10}^{-4}---\left(51\right)$

③滤波初始条件估算

假定系统一致完全随机可控且一致完全随机可观，则卡尔曼滤波器具有一致渐近稳定性，随着滤波步数的增加，状态初值x₀和误差协方差矩阵初值P₀对状态值x_k和P_k的影响将逐渐消失，使估计值趋向无偏，x₀和P₀的选择范围较为宽松；因此，选择初始状态x₀为单位四元数(1,0,0,0)，选择P₀初始值为单位阵。