本发明公开了一种基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置和方法，包括MEMS传感器、INS模块、VLC定位模块、PDR定位模块和测姿定位单卡尔曼滤波器模块；本发明基于INS惯导机械编排的误差方程作为融合滤波器的系统方程，观测方程包括VLC定位信息更新、PDR定位信息更新和磁力计观测量更新。融合滤波器输出VLC接收器的姿态给VLC定位模块，输出PDR设备的姿态给PDR定位模块以校正姿态的影响。首次在VLC定位领域使用融合测姿准确估计VLC接收器的姿态信息，主要解决了VLC定位容易受设备姿态影响以及在光信号被遮挡情况下定位不连续的问题。
1.基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置，其特征在于：包括MEMS传感器、INS模块、VLC定位模块、PDR定位模块和测姿定位单卡尔曼滤波器模块；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；
所述测姿定位单卡尔曼滤波器模块的输入包括：加速度计测得的接收器在XYZ方向的加速度信息和陀螺仪测得的接收器在XYZ方向上的角速度信息传递给INS模块，由INS模块对其进行编排得到接收器的INS位置、速度和姿态信息；磁力计测得的接收器相对东、南、西、北方向的角度信息；加速度计测得的接收器在XYZ方向的加速度信息给PDR定位模块进行位置估计得到的PDR定位信息和VLC定位模块输出的VLC定位信息通过加权平均后的位置信息；
所述测姿定位单卡尔曼滤波器模块输出当前时刻接收器的位置信息、速度信息和姿态信息给INS模块，输出当前时刻接收器的姿态信息给PDR定位模块或VLC定位模块，输出噪声补偿信号反馈给加速度计和陀螺仪，同时输出当前时刻接收器的定位信息。
2.根据权利要求1所述的导航装置，其特征在于：所述PDR定位信息和VLC定位信息的权重由VLC定位模块中光信号是否被遮挡来设定，若光信号被遮挡时系统将VLC位置信息的权重设为0，只采用PDR位置信息。
3.一种基于权利要求1或2所述导航装置的导航方法，其特征在于，包括以下步骤：(1)建立测姿定位单卡尔曼滤波器模块S-EKF的状态向量；
(2)建立测姿定位单卡尔曼滤波器模块S-EKF的系统模型；
(3)建立测姿定位单卡尔曼滤波器模块S-EKF的观测方程；
(4)测姿定位单卡尔曼滤波器模块S-EKF滤波输出定位信息。
4.根据权利要求3所述的一种导航方法，其特征在于：所述步骤(1)中S-EKF的状态向量为：x＝[δrn δvn ψ bg ba]T
其中，δrn、δvn、ψ、bg和ba分别是接收器的位置误差矢量、速度误差矢量、姿态误差矢量、陀螺仪偏差矢量和加速度计偏差矢量。
5.根据权利要求4所述的一种导航方法，其特征在于，所述步骤(2)包括：(21)将加速度计和陀螺仪采集到的接收器数据信息输入到INS模块进行机械编排算法处理得到接收器当前位置信息、速度信息和姿态信息并输入到S-EKF中；对姿态矩阵进行坐标变换，其坐标转移方程为：其中， 为rn的一阶导数， 是导航坐标系中的位置向量，表示纬度，λ表示n n n
经度和h表示高度； 是v的一阶导数，v 是三维速度向量；g是导航坐标系中的重力向量；
表示位置增量； 表示速度增量；fb是载体坐标系中的特定力矢量；D-1是一个关于 和h的
3×3矩阵； 是 的一阶导数， 是由INS机械编排预测的从载体坐标系到导航坐标系的方向余弦矩阵； 和 分别为角速度矢量 和 的斜对称矩阵；而和 表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度； 和表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；
(22)S-EKF的系统模型为 具体展开公式如下：其中，δ符号表示误差，即真实值和系统标称值之差； 和 分别表示n n n
δr、δv 、ψ、bg和ba的一阶导数，f是投影到导航坐标系的特定力矢量；wg和wa是传感器噪声；
τbg和τba代表惯导噪声的相关时间；wbg和wba是驱动噪声，符号“×”表示叉乘。
6.根据权利要求5所述的一种导航方法，其特征在于，所述步骤(3)包括：(31)PDR或VLC输出的位置观测方程
当发生光信号遮挡VLC无法输出有效定位信息的情况时，PDR位置信息作为测姿定位单卡尔曼滤波器模块的输入量；根据PDR位置信息建立的观测方程为：其中 和 为INS机械编排算出的纬度和经度； 和 分别为来自PDR的纬度和经度； 和nλ是测量噪声；
否则，测姿定位单卡尔曼滤波器模块接收VLC位置信息，而忽略PDR位置信息；根据VLC输出信息建立的观测方程为：其中， 和 分别是来自于INS机械编排和VLC的位置向量；δrn是位置误差矢量；n1是量测噪声；
滤波器直接通过磁力计读数进行姿态更新，磁力计观测方程如下：其中， 是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声。
7.根据权利要求6所述的一种导航方法，其特征在于，所述步骤(4)包括：(41)将系统模型 根据卡尔曼滤波的模型形式进行变换成如下形式：其中，
其中， H＝[I3×3 03×3 03×3 03×3 03×3]，V＝[n1]；
当VLC输出无效位置信息时，采用PDR输入的位置信息，观测方程为：当采用VLC位置信息时，观测方程为：
当采用磁力计观测方程滤波时，将磁力计观测方程依Z＝HX+V形式变换后参与滤波，其中， H＝[03×3 03×3 [mn×] 03×3 03×3]，(43)扩展卡尔曼滤波递推步骤如下：
P0＝diag([var(r0) var(v0) var(ψ0) var(bg0) var(ba0)])其中，r0、v0、ψ0、bg0和ba0分别表示向量δrn、δvn、ψ、bg和ba的初始值，P0表示各个向量初始向量的协方差构成的对角矩阵；
(b)利用k-1时刻的对角矩阵对k时刻的对角矩阵进行预测，预测公式为：P′k＝FPk-1FT+Q
其中，Pk-1表示k-1时刻的对角矩阵，P′k表示k时刻的对角矩阵的预测值，Q＝E[(GW)(GW)T]；
(c)利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：其中， 表示k-1时刻的状态量， 表示k时刻状态量的预测值；
(d)计算k时刻的卡尔曼增益K′，计算公式如下：K′k＝P′kHT(HP′kHT+R)-1
其中，K′k表示k时刻的卡尔曼增益，R＝E[VV]；
(e)利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量 则：(f)利用k时刻的卡尔曼增益估计k时刻的对角矩阵的最优值，令该最优值作为k时刻的对角矩阵Pk，则：Pk＝(I-KkH)P′k；
(g)输出k时刻状态量中的姿态信息反馈给VLC定位模块或PDR定位模块，输出k时刻状态量中的位置信息、速度信息和姿态信息反馈给INS模块，输出k时刻状态量中陀螺仪偏差矢量反馈给陀螺仪，输出k时刻状态量中加速度计偏差矢量反馈给加速度计；并输出k时刻状态量中接收器的定位信息；同时，令k＝k+1，返回步骤(b)继续循环滤波不断更新状态量，输出定位信息。
基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置和
方法
技术领域
[0001] 本发明涉及智能定位装置及方法，特别是涉及基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置和方法。
背景技术
[0002] 随着室内定位技术的发展和应用，基于可见光通信的室内定位技术也在迅速兴起并得到广泛关注。在充足室内光源的环境下，通过光传感器等设备检测得到经过复用协议调制的光信号，通过信号解调技术可以将不同光源信号数据分离，从而结合环境参数可以计算出定位目标相对每个光源的距离或角度信息，最后通过定位算法如三边定位可以完成目标定位。
[0003] 然而，由于目标接收设备的姿态会随着目标移动产生抖动，将对VLC定位结果带来较大的影响。另一方面，实际场景中光信号容易被遮挡，将会导致定位不连续。针对前一个问题，目前的方案主要是通过多传感器组合共同定位。针对后一个问题，主流的解决方案是通过卡尔曼滤波或者粒子滤波来估计目标位置。
[0004] 但这些方案存在一些问题：1)相比于单传感器定位，多传感器组合定位算法复杂且成本较高；2)目前定位方案中提出的滤波器融合是以探测器姿态平稳为前提，在实际场景中稳定性较差；3)在信号被遮挡的情况频繁发生的场景中，仅通过VLC数据加滤波器的定位系统推测的目标位置仍然与实际位置有较大偏差，且VLC定位结果不连续、不平滑。
发明内容
[0005] 发明目的：为解决现有技术的不足，提供一种可消除姿态对VLC定位的影响，可弥补VLC定位结果不连续、不平滑的缺点的基于MEMS传感器和VLC定位融合的测姿定位单卡尔曼滤波器导航装置和方法。
[0006] 技术方案：基于MEMS传感器和VLC定位融合的测姿定位单卡尔曼滤波器导航装置，包括MEMS传感器、惯性导航系统(Inertial Navigation System，INS)模块、可见光通信(Visible Light Communication，VLC)定位模块、行人航位推算(Pedestrian Dead Reckoning，PDR)定位模块和测姿定位单卡尔曼滤波器模块；所述MEMS传感器包括加速度计、陀螺仪以及磁力计；
[0007] 所述测姿定位单卡尔曼滤波器模块的输入包括：加速度计测得的接收器在XYZ方向的加速度信息和陀螺仪测得的接收器在XYZ方向上的角速度信息传递给INS机械编排模块，由INS模块对其进行编排得到接收器的INS位置、速度和姿态信息；磁力计测得的接收器相对东、南、西、北方向的角度信息；加速度计测得的接收器在XYZ方向的加速度信息给PDR定位模块进行位置估计得到的PDR定位信息和VLC定位模块输出的VLC定位信息通过加权平均后的位置信息；
[0008] 所述测姿定位单卡尔曼滤波器模块输出当前时刻接收器的位置信息、速度信息和姿态信息给INS机械编排模块，输出当前时刻接收器的姿态信息给PDR定位模块或VLC定位模块，输出噪声补偿信号反馈给加速度计和陀螺仪，同时输出当前时刻接收器的定位信息。
[0009] 其中，所述PDR定位信息和VLC定位信息的权重由VLC定位模块中光信号是否被遮挡来设定，若光信号被遮挡时系统将VLC位置信息的权重设为0，只采用PDR位置信息。
[0010] 一种基于所述导航装置的导航方法，包括以下步骤：
[0011] (1)建立S-EKF的状态向量；
[0012] (2)建立S-EKF的系统模型；
[0013] (3)建立S-EKF的观测方程；
[0014] (4)S-EKF滤波输出定位信息。
[0015] 进一步的，所述步骤(1)中S-EKF的状态向量为：
[0016] x＝[δrn δvn ψ bg ba]T
[0017] 其中，δrn、δvn、ψ、bg和ba分别是接收器的位置误差矢量、速度误差矢量、姿态误差矢量、陀螺仪偏差矢量和加速度计偏差矢量。
[0018] 进一步的，所述步骤(2)包括：
[0019] (21)将加速度计和陀螺仪采集到的接收器数据信息输入到INS模块进行机械编排算法处理得到接收器当前位置信息、速度信息和姿态信息并输入到S-EKF中；对姿态矩阵进行坐标变换，其坐标转移方程为：
[0020]
[0021] 其中， 为rn的一阶导数， 是导航坐标系中的位置向量，表示纬度，λ表示经度和h表示高度； 是vn的一阶导数，vn是三维速度向量；gn是导航坐标系中的重力b -1
向量； 表示位置增量； 表示速度增量；f 是载体坐标系中的特定力矢量；D 是一个关于和h的3×3矩阵； 是 的一阶导数， 是由INS机械编排预测的从载体坐标系到导航坐标系的方向余弦矩阵； 和 分别为角速度矢量 和 的斜对称矩
阵；而 和 表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度； 和 表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度；
[0022] (22)A-EKF的系统模型为 具体展开公式如下：
[0023]
[0024] 其中，δ符号表示误差，即真实值和系统标称值之差； 和 分n n n
别表示δr、δv、ψ、bg和ba的一阶导数，f是投影到导航坐标系的特定力矢量；wg和wa是传感器噪声；τbg和τba代表惯导噪声的相关时间；wbg和wba是驱动噪声，符号“×”表示叉乘。
[0025] 进一步的，所述步骤(3)包括：
[0026] (31)PDR或VLC输出的位置观测方程
[0027] 当发生光信号遮挡VLC无法输出有效定位信息的情况时，PDR位置信息作为测姿定位单卡尔曼滤波器模块的输入量；根据PDR位置信息建立的观测方程为：
[0028]
[0029] 其中 和 为INS机械编排算出的纬度和经度； 和 分别为来自PDR的纬度和经度； 和nλ是测量噪声；
[0030] 否则，测姿定位单卡尔曼滤波器模块接收VLC位置信息，而忽略PDR位置信息；根据VLC输出信息建立的观测方程为：
[0031]
[0032] 其中， 和 分别是来自于INS机械编排和VLC的位置向量；δrn是位置误差矢量；n1是量测噪声；
[0033] (32)磁力计观测方程
[0034] 滤波器直接通过磁力计读数进行姿态更新，磁力计观测方程如下：
[0035]
[0036] 其中， 是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声。
[0037] 进一步的，所述步骤(4)包括：
[0038] (41)将系统模型 根据卡尔曼滤波的模型形式进行变换成如下形式：
[0039]
[0040] 其中，
[0041]
[0042]
[0043] (42)观测模型Z＝HX+V对应以下方程：
[0044]
[0045] 其中， H＝[I3×3 03×3 03×3 03×3 03×3]，V＝[n1]；
[0046] 当VLC输出无效位置信息时，采用PDR输入的位置信息，观测方程为：
[0047]
[0048] 当采用VLC位置信息时，观测方程为：
[0049]
[0050] 当采用磁力计观测方程滤波时，将磁力计观测方程依Z＝HX+V形式变换后参与滤波，其中， H＝[03×3 03×3[mn×]03×3 03×3]，
[0051] (43)扩展卡尔曼滤波递推步骤如下：
[0052] (a)设置初值
[0053] 令协方差矩阵P的初始值为：
[0054] P0＝diag([var(r0) var(v0) var(ψ0) var(bg0) var(ba0)])
[0055] 其中，r0、v0、ψ0、bg0和ba0分别表示向量δrn、δvn、ψ、bg和ba的初始值，P0表示各个向量初始向量的协方差构成的对角矩阵；
[0056] 设状态量X初始值为0向量；
[0057] (b)利用k-1时刻的对角矩阵对k时刻的对角矩阵进行预测，预测公式为：
[0058] Pk′＝FPk-1FT+Q
[0059] 其中，Pk-1表示k-1时刻的对角矩阵，Pk′表示k时刻的对角矩阵的预测值，Q＝E[(GW)(GW)T]；
[0060] (c)利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
[0061]
[0062] 其中， 表示k-1时刻的状态量， 表示k时刻状态量的预测值；
[0063] (d)计算k时刻的卡尔曼增益K′，计算公式如下：
[0064] Kk′＝Pk′HT(HPk′HT+R)-1
[0065] 其中，Kk′表示k时刻的卡尔曼增益，R＝E[VVT]；
[0066] (e)利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量 则：
[0067]
[0068] (f)利用k时刻的卡尔曼增益估计k时刻的对角矩阵的最优值，令该最优值作为k时刻的对角矩阵Pk，则：
[0069] Pk＝(I-KkH)Pk′；
[0070] (g)输出k时刻状态量中的姿态信息反馈给VLC定位模块或PDR定位模块，输出k时刻状态量中的位置信息、速度信息和姿态信息反馈给INS模块，输出k时刻状态量中陀螺仪偏差矢量反馈给陀螺仪，输出k时刻状态量中加速度计偏差矢量反馈给加速度计；并输出k时刻状态量中接收器的定位信息；同时，令k＝k+1，返回步骤(b)继续循环滤波不断更新状态量，输出定位信息。
[0071] 有益效果：与现有技术相比，本发明的基于MEMS传感器和VLC定位融合的单卡尔曼滤波导航装置和方法具有以下优点：1)首次在VLC定位领域使用融合测姿准确估计VLC接收器的姿态信息，并消除姿态对VLC定位的影响；2)可弥补VLC定位结果不连续、不平滑的缺点；3)可在VLC信号被遮挡的情况下使用MEMS传感器信息提供定位结果，且算法效率高，成本低；
附图说明
[0072] 图1是测姿定位单卡尔曼滤波器导航装置结构示意图；
[0073] 图2是俯仰角0°的仿真定位结果CDF曲线；
[0074] 图3是俯仰角5°的仿真定位结果CDF曲线；
[0075] 图4是俯仰角8°的仿真定位结果CDF曲线；
[0076] 图5是接收设备平放、倾斜5°、倾斜8°时的定位误差分析结果。
具体实施方式
[0077] 下面结合附图对本发明的技术方案进行详细的说明。
[0078] 如图1所示，基于MEMS传感器和VLC定位融合的测姿定位单卡尔曼滤波器导航装置包括：MEMS传感器、惯性导航系统(Inertial Navigation System，INS)模块、可见光通信(Visible Light Communication，VLC)定位模块、行人航位推算(Pedestrian Dead Reckoning，PDR)定位模块和测姿定位单卡尔曼滤波器模块(S-EKF)，其中MEMS传感器包括加速度计、陀螺仪和磁力计。
[0079] 加速度计测量接收器在XYZ方向的加速度信号一路传递给PDR定位模块进行位置估计，另一路传递给INS模块，陀螺仪测量的接收器在XYZ方向的角速度信号传递给INS机械编排模块。INS模块通过机械编排算法对加速度信号和角速度信号进行机械编排，得到INS位置信息、速度信息和姿态信息。PDR定位模块对加速度信号进行位置估计，得到PDR位置信息。磁力计测量接收器相对东、南、西、北方向的角度信息，VLC定位模块测到接收器的位置信息。
[0080] 另外，VLC定位模块和PDR定位模块虽然同时进行定位，但各自的定位结果并不是等价输入到测姿定位单卡尔曼滤波器模块中。一般情况下，测姿定位单卡尔曼滤波器模块主要接受VLC定位模块的位置信息，而忽略PDR定位模块的位置信息。只有当发生光信号遮挡等VLC定位模块无法输出有效定位信息的情况时，PDR模块的位置信息才会作为主要的输入量。
[0081] 即，将PDR定位模块的位置信息和VLC定位模块的位置信息通过加权平均后的位置信息作为唯一的位置信息输入到测姿定位单卡尔曼滤波器模块，当VLC定位模块中光信号被遮挡时系统将VLC定位模块的位置信息的权重设为0，只采用PDR模块的位置信息。
[0082] PDR位置信息、VLC位置信息、磁力计测得的角度信息以及INS位置信息、速度信息和姿态信息一起作为输入量输入到测姿定位单卡尔曼滤波器模块，经过滤波器处理输出当前时刻接收器的位置信息、速度信息和姿态信息反馈给INS模块，输出当前时刻接收器的姿态信息反馈给PDR模块或VLC定位模块，输出陀螺仪偏差矢量反馈给陀螺仪，输出加速度计偏差矢量反馈给加速度计，并输出当前时刻接收器的定位信息，同时更新下一时刻的状态量。
[0083] 测姿定位测姿定位单卡尔曼滤波器(Single Extended Kalman Filter，S-EKF)的重点在于状态向量、系统方程、状态方程的建模。S-EKF主要用来估计接收器的姿态信息(即：横滚角、俯仰角和方位角)和位置信息(即：经度、纬度和高度)。
[0084] 一种基于上述导航装置的导航方法，包括以下步骤：
[0085] (1)建立S-EKF的状态向量
[0086] S-EKF的状态向量定义为：
[0087] x＝[δrn δvn ψ bg ba]T (1)
[0088] 其中，δrn、δvn、ψ、bg和ba分别是接收器的位置误差矢量、速度误差矢量、姿态误差矢量、陀螺仪偏差矢量和加速度计偏差矢量。
[0089] (2)建立S-EKF的系统模型
[0090] (21)将加速度计和陀螺仪采集到的接收器数据信息输入到INS模块进行机械编排算法处理得到接收器当前位置信息、速度信息和姿态信息并输入到S-EKF中；对姿态矩阵进行坐标变换，其坐标转移方程为：
[0091]
[0092] 其中， 为rn的一阶导数， 是导航坐标系中的位置向量，表示纬度，λ表示经度和h表示高度； 是vn的一阶导数，vn是三维速度向量；gn是导航坐标系中的重力向量； 表示位置增量； 表示速度增量；fb是载体坐标系中的特定力矢量；D-1是一个关于和h的3×3矩阵； 是 的一阶导数， 是由INS机械编排预测的从载体坐标系到导航坐标系的方向余弦矩阵； 和 分别为角速度矢量 和 的斜对称矩
阵；而 和 表示地球自传的角速度和导航坐标系相对于地心地固坐标系的转动角速度； 和 表示载体坐标系相对于惯性坐标的转动角速度和导航坐标系相对于惯性坐标的转动角速度。
[0093] (22)A-EKF的系统模型为 具体展开公式如下：
[0094]
[0095] 其中，δ符号表示误差，即真实值和系统标称值之差； 和 分n n n
别表示δr、δv、ψ、bg和ba的一阶导数，f是投影到导航坐标系的特定力矢量；wg和wa是传感器噪声；τbg和τba代表惯导噪声的相关时间；wbg和wba是驱动噪声，符号“×”表示叉乘。
[0096] (3)建立S-EKF的观测方程
[0097] S-EKF的观测方程由两部分组成：1)PDR或VLC输出的位置观测方程2)磁力计观测方程。需要注意的是，应当根据采用PDR位置信息作为输入或VLC作为输入而采用不同的观测方程。
[0098] (31)PDR或VLC输出的位置观测方程
[0099] 当发生光信号遮挡VLC无法输出有效定位信息的情况时，PDR的位置信息作为测姿定位单卡尔曼滤波器模块主要的输入量；根据PDR位置建立的观测方程为：
[0100]
[0101] 其中 和 为INS机械编排算出的纬度和经度； 和 来自PDR的纬度和经度； 和nλ是测量噪声。
[0102] 否则，测姿定位单卡尔曼滤波器模块主要接收VLC的位置信息，而忽略PDR的定位信息；根据VLC输出信息建立的观测方程为：
[0103]
[0104] 其中， 和 分别是来自于INS机械编排和VLC的位置向量；δrn是位置误差矢量；n1是量测噪声。
[0105] 所述PDR定位信息和VLC定位信息的权重由VLC定位模块中光信号是否被遮挡来设定，若光信号被遮挡时系统将VLC位置信息的权重设为0，只采用PDR位置信息。
[0106] (32)磁力计观测方程
[0107] 滤波器直接通过磁力计读数进行姿态更新，磁力计观测方程如下：
[0108]
[0109] 其中， 是磁力计读数矢量，mn是校准的LMF矢量，n3是噪声。
[0110] (4)S-EKF滤波
[0111] 为了进一步实现扩展卡尔曼滤波，需要将系统模型(公式(3))和观测模型(公式(4)～(6))代入到扩展卡尔曼滤波的时间更新方程和测量更新方程。首先先将上述中系统模型和观测模型根据卡尔曼滤波的模型形式进行变换，从而与模型中的变量一一对应。
[0112] (41)将系统模型： 该形式可通过公式(3)变换而来变换后的方程如下：
[0113]
[0114] 其中，
[0115]
[0116]
[0117] (42)观测模型：Z＝HX+V，对应以下方程：
[0118]
[0119] 其中， H＝[I3×3 03×3 03×3 03×3 03×3]，V＝[n1]；
[0120] 当VLC输出无效位置信息时，采用PDR输入的位置信息，观测方程为：
[0121]
[0122] 当采用VLC的位置信息时，观测方程为：
[0123]
[0124] 当采用磁力计观测方程滤波时，同样需要将磁力计观测方程(6)依Z＝HX+V形式变换后参与滤波。其中， H＝[03×3 03×3[mn×]03×3 03×3]，
[0125] (43)扩展卡尔曼滤波
[0126] 确定好各个变量以后，就可以按照扩展卡尔曼滤波的时间更新方程和测量更新方程递推进行滤波。扩展卡尔曼滤波递推步骤如下：
[0127] (a)设置初值
[0128] 令协方差矩阵P的初始值为：
[0129] P0＝diag([var(r0) var(v0) var(ψ0) var(bg0) var(ba0)]) (11)
[0130] 其中，r0、v0、ψ0、bg0和ba0分别表示向量δrn、δvn、ψ、bg和ba的初始值，P0表示各个向量初始向量的协方差构成的对角矩阵。
[0131] 设状态量X初始值为0向量。
[0132] (b)利用k-1时刻的对角矩阵对k时刻的对角矩阵进行预测，预测公式为：
[0133] Pk′＝FPk-1FT+Q (12)
[0134] 其中，Pk-1表示k-1时刻的对角矩阵，Pk′表示k时刻的对角矩阵的预测值，Q＝E[(GW)(GW)T]。
[0135] (c)利用k-1时刻的状态量对k时刻的状态量进行预测，预测公式为：
[0136]
[0137] 其中， 表示k-1时刻的状态量， 表示k时刻状态量的预测值。
[0138] (d)计算k时刻的卡尔曼增益K′，计算公式如下：
[0139] Kk′＝Pk′HT(HPk′HT+R)-1 (14)
[0140] 其中，Kk′表示k时刻的卡尔曼增益，R＝E[VVT]。
[0141] (e)利用k时刻的卡尔曼增益估计k时刻的状态量的最优值，令该最优值作为k时刻的状态量 则：
[0142]
[0143] (f)利用k时刻的卡尔曼增益估计k时刻的对角矩阵的最优值，令该最优值作为k时刻的对角矩阵Pk，则：
[0144] Pk＝(I-KkH)Pk′ (16)
[0145] (g)输出k时刻状态量中的姿态信息反馈给VLC定位模块和PDR定位模块，输出k时刻状态量中的位置信息、速度信息和姿态信息反馈给INS模块，输出k时刻状态量中陀螺仪偏差矢量反馈给陀螺仪，输出k时刻状态量中加速度计偏差矢量反馈给加速度计；并输出k时刻状态量中接收器的定位信息；
[0146] (h)令k＝k+1，返回步骤(b)继续循环滤波不断更新状态量，输出定位信息。
[0147] 其中，步骤(b)、(c)中的方程为时间更新方程，(d)、(e)、(f)中方程为测量更新方程。
[0148] 滤波过程除了初始时为(a)～(g)进行，从第二次循环起可以表示为步骤(b)～(h)循环进行，随时间不断更新状态量，而状态中的姿态信息会反馈给VLC定位模块和PDR定位模块，以矫正姿态的影响，定位信息(位置信息和速度信息)会向外输出。由于状态向量是位置误差向量和速度误差向量，因此需要将输出位置信息那一时刻的INS位置速度信息加上滤波器输出的误差向量，从而变为位置和速度进行输出。
[0149] 该融合滤波器(测姿定位单卡尔曼滤波器)首次在VLC定位领域使用融合测姿准确估计接收器的姿态信息，并消除姿态对VLC定位的影响。基于惯导机械编排的误差方程作为融合滤波器的系统方程。而观测方程则包括加速度计观测量更新(PDR或VLC输出的位置观测方程)和磁力计观测量更新。融合滤波器输出接收器的姿态给VLC定位模块以校正姿态的影响。在定位滤波器的设计中，二维平面的位置信息作为系统状态向量，基于行人航位推算的误差方程作为系统方程，而VLC的定位结果则作为定位滤波器的观测方程。
[0150] 技术原理：
[0151] 在VLC定位模块中，定位算法一般采用三边定位算法。其原理是利用室内顶端的多盏灯作为发射源，一般至少有三盏LED灯，通过接收器实时测得的光信号强度代入光信号传播模型估计出目标点到每盏灯的距离，最后通过联立方程估算出定位位置。然而，当光信号被遮挡时，探测器无法获取全部信号，求解出的位置会严重超出误差范围，这种情况会通过阈值进行排除，所以系统在该情况下不输出位置信息，导致定位结果不连续、不平滑。另外，考虑到VLC系统往往存在散粒噪声和热噪声等随机噪声，距离的估计往往存在一定误差，因此估得的位置形成的运动轨迹会存在陡峭变化等特征，这往往不符合实际目标的运动特征。由于在目标跟踪时，先验知识应当表示定位轨迹平滑，目标当前时刻的状态与上一时刻的状态相关，而滤波方法可以将先验值考虑进来，从而使得定位轨迹更加平滑。
[0152] 目前一般的VLC定位往往假设设备是平放姿态，从而简化了定位算法。然而在实际情况中设备会产生抖动、倾斜。设备的随机倾斜会改变光信号的入射角，从而导致测得的光信号强度发生变化。如果无法准确估计设备的姿态，这部分的光信号强度变化将成为系统误差的一部分，导致定位误差变大。为了说明姿态误差对于VLC定位精度的影响程度，我们通过对比仿真了不同俯仰角的情况下VLC定位的精度。该仿真实验对接收器发生倾斜，但定位算法不校正的情况进行了模拟，对所有接收器仿真了两种不同的倾斜情况，定位结果CDF曲线分别如图3和图4所示，图2表示设备未发生倾斜的定位结果。实验结果显示接收器俯仰角为5°和8°时最大定位误差分别约为0.13m和0.21m。对比接收器倾斜和平放时的定位结果，如图5所示，可以看出当接收器发生了倾斜，如果不对接收器姿态进行修正，定位误差会显著的增大。该仿真结果证明准确估计接收器的姿态对提高可见光定位系统的性能具有重要意义。因此，引入MEMS姿态估计模块，将准确的姿态信息传递给VLC定位模块进行姿态校准，能够显著的减小姿态对VLC定位结果的影响。
[0153] 由于光信号在被遮挡的情况下VLC定位无法进行，因此引入PDR定位模块进行定位。PDR定位可以完全利用MEMS传感器信号来计算步长和方向，利用行人上一时刻的位置推测出行人当前的位置。通过反复迭代，即可得到行人的运动轨迹。因此，当VLC定位发生信号遮挡时，无法输出位置结果，融合系统会采用PDR定位模块的定位结果进行弥补。
