运动跟踪算法
人体的运动可以分解为躯干、头部、四肢的运动，通常在关节两侧的肢体有明显的运动模式差异。在本研究所述系统里，受限于传感器数量和穿戴的复杂程度，我们主要监控受试者躯干、四肢大关节附近的肢体运动，而忽略头部、四肢末端的运动。此外，躯干部分也假设为一个整体，不考虑脊柱的屈伸变化。
在多传感器数据融合的运动捕获算法里，传感器的佩戴位置对算法设计有很大影响。前臂和上臂可以在3个自由度里自由变化，且速度较快，方向无法预测；大腿和小腿的运动主要是屈伸方向和朝向的变化，向两侧的偏转（滚转）较少出现且幅度较小；躯干部分则是以平动为主，大幅度的前倾、后仰、侧向弯曲较少出现，且动作较慢。这里以放置于大腿部位的传感器为例介绍本系统所实施的动作捕获算法。
大腿部位的传感器佩戴于身体侧向平面。启动时测量x、y、z轴三个方向的加速度、角速度和磁力，其中z方向为垂直纸面向外。髋关节所允许的大腿运动方向包括屈伸方向（俯仰角）和外展/内收方向（滚转角）。此外，由于人体朝向可任意变化，大腿的航向角也会随时变化。在这三个角度里，航向角可以在大腿伸直的状况下由y轴磁力计直接读取，无需滤波计算。对于导致滚转角变化的外展/内收方向的运动，康复医生认为此类动作的幅度较小，且速度很慢，无需特别跟踪。在我们的算法里通过重力在y轴的分量进行三角运算得出。大腿最主要的运动是屈伸，屈伸将导致俯仰角发生变化，我们采用卡尔曼滤波对其进行计算。
如图6-21的θ所示，大腿的屈伸角定义为y轴与重力反方向的夹角，大腿的摆动速度（角速度）为v=dθ/d t。图中c表示水平方向加速度，a表示竖直方向加速度，θ、v、a、c都作为卡尔曼滤波跟踪的变量。考虑到大腿的摆动速度较慢，且系统采样频率为25Hz，在采样间隔内，角速度的变化很小（变化值可通过系统噪声模拟），因此滤波器使用常角速度模型进行推演。状态更新方程见式（6-1）。
图6-21 大腿处传感器的坐标轴方向与参数定义
下标k和k-1表示k时刻和k-1时刻，ts为采样间隔，wθ、wv、wa和wc分别为各个状态变量的系统噪声。可以假设它们为独立零均值高斯噪声，其分布函数见式（6-2）：
式中：
Qθ、Qv、Qa和Qc是各个状态变量的方差。传感器的测量值为x轴方向加速度d和y轴方向加速度b，观测值和状态变量的关系（观测方程）见式（6-4）。
式中，wd和wb是观测噪声，r是髋关节到传感器之间的距离。同样假设观测噪声是独立零均值的高斯噪声，则：
式中，
是观测噪声的协方差矩阵，Rb和Rd是测量值的方差。
式（6-4）写成矩阵格式为：
式（6-4）和式（6-5）里的观测方程不是线性的，无法直接进行卡尔曼方程推导，需对其进行局部线性化，将函数h对各状态变量求偏导，即
，得到式（6-7）：
(https://www.daowen.com)
局部线性化按式（6-8）进行：
式中，
和
是dk和bk的估计值
分别是θk、vk、ak、ck的估计值。
获取状态变量后验预测值后，按式（6-9）更新状态变量估计值的协方差矩阵。
以上步骤是卡尔曼滤波的递归推导。在实际使用时，需预估状态变量、系统噪声、观测噪声和协方差矩阵（初始化参数设置及递归计算实例可参阅）。以k-1时刻为例，根据状态方程对k时刻的先验预测见式（6-10）。等式右侧带有波浪线的参数为k-1时刻的滤波估计值，下标（k|k-1）表示从k-1到k时刻的单步预测。
同时对协方差矩阵进行单步预测，见式（6-11）：
式中，
，为式（6-1）的状态转移矩阵。
观测值的单步预测见式（6-12）：
卡尔曼增益由多个协方差矩阵和经局部线性化处理的观测函数导出，见式（6-13）所示：
如式（6-14）所示，k时刻状态变量的估计值由两部分估计值经卡尔曼增益加权后得出。
最后，对状态变量估计值的协方差矩阵进行更新，为k+1时刻的滤波做好准备。
以上是对大腿俯仰角跟踪的算法，此算法同样适用于运动模式类似的小腿。根据康复医生的建议，躯干部位的俯仰运动和滚转运动幅度较小，且变化较慢，系统根据重力分量在各个轴上的分量经三角运算获得，躯干的方位角也由磁力计分量的三角运算获取。前臂和上臂的动作较为复杂，系统中使用与上面类似的卡尔曼滤波进行跟踪，原理相同，但状态更新方程和观测方程里的变量和矩阵的维度都大幅增加。跟踪算法中所采用的卡尔曼滤波是一种无系统延迟的滤波算法，在k时刻获得新的测量值后即可获取当前状态参数的滤波值，对于目前运算能力较为强大的微处理器，数据滤波可在一个采样间隔内完成。
在本系统里，运动捕获是由传感器内部的嵌入式程序完成的，也就是说，每一个传感器输出至网关的不是原始加速度、角速度和磁力值，而是滤波得到的所附肢体的滚转角、航向角和俯仰角。每个受试者节点的9个传感器以分布计算的方式得出姿态角，发送至网关进行汇总。这种方式大大降低了网关的运算压力，尤其是当一个网关与多个节点建立通信联系时，这样的分布计算方式就显得格外重要。