将平滑处理后的CFP值和传 感器数据采集和处理模块2所获取到的待测目标的第二位置信息进行融合得到待测目标的 最终位置信息。
[0088] 上述组合滤波器31可以根据统计需求进行预先设定，可以采用扩展卡尔曼滤波 (ExtendedKalmanFilter，EKF)、无损卡尔曼滤波（UnscentedKalmanFilter，UKF)或粒 子滤波（ParticleFilter，PF) 〇
[0089] a.扩展卡尔曼滤波
[0090] 如果采用高斯统计，可以采用EKFiKF的15元状态误差向量在K时刻可以表示为：
[0091]
[0092] 3rk，Svk，和分别是位置，速度，和姿态误差。bk和dk是加速度偏差和陀螺仪漂 移。EKF为了积分线性化了非线性系统模型，并且线性化模型如下：
[0093] 5xk+i|k= ?k,k+i5xk|k+?k (15)
[0094] 其中SXk+l|k是误差状态预测，SXk|k是在前一刻K时的误差状态，cok是过程噪声，它 的协方差矩阵为炻=￡(叫<)，Φk,k+i是一个15X15的转移矩阵：
[0095]
[0096]其中△t是两个时刻的差。略是在导航框架中加速度矢量的矫正：
[0097]
[0098] 是C的斜对称矩阵：
[0099]
[0100] 其中J给出如下：
[0101]
[0102] 其中R是地球半径，h是高度，?>是炜度。EKF的测量模型如下：
[0103] zk = Hk5xk|k+Uk (20)
[0104] 其中zk是测量误差向量，叫是测量噪声，它的协方差矩阵是KHk是参数 .〇 矩阵定义如下：
[0105]Hk=[l3X303xi2] (21)
[0106] 最后，等式(29)和等式(30)被用来估计状态误差向量。状态误差向量被用来更新 当前的组合导航方案。组合导航方案将会提供导航信息，例如位置和速度包括它们的方差 用于无线指纹识别和平滑滤波器。整个方案是一个稳定的闭环。
[0107] b.其他过滤器
[0108] UKF无损卡尔曼滤波(UnscentedKalmanFilter，UKF)假设系统存在高斯误差，并 且选择的点服从高斯分布。UKF与EKF相比较优点是利用点来粗略估计均值和协方差参数， 估计的均值和协方差比EKF要好，尤其是对高阶非线性系统。
[0109]如果系统误差为非高斯误差，组合滤波器31可以采用粒子滤波（Particle FiIter，PF)，PF使用基于蒙特卡洛模拟的递归贝叶斯估计。在PF的实现中，许多带有权重的 粒子被产生以代表条件概率密度函数，这些粒子通过系统模型传播，PF在非高斯误差的系 统中性能优于UKF和EKF。然而粒子的数量是巨大的，因此PF有着很大的计算负载。
[0110]平滑滤波器32用来平滑距离数据处理模块1所获取到的约束指纹识别(ConstrainedFingerprinting，CFP)的解，由于CFP解并不平滑，还有一些跳跃点，利用平 滑滤波器32进一步提高整个系统的精度。平滑滤波器32可以采用卡尔曼滤波（Kalman Filter，KF)或自适应卡尔曼滤波（AdaptiveKalmanFilter，AKF)，AKF会自适应地根据导 航方案调整滤波器参数。
[0111] a.KF滤波器
[0112] KF滤波器可用于实现平滑滤波器。KF的状态向量定义如下：
[0113] x=[reΓη Ve Vn]T(22)
[0114] 式中和rn是水平面上的二维位置组成（如东和北），ve和vn是对应的二维速度组 成。平滑滤波器工作在二维模式。KF系统模型采用经典的运动模型，定义如下：
[0115] xk+i|k=〇k,k+ixk|k+?k (23)
[0116] 式中xk+i|k是预测的状态向量，xk|k是在k时刻先前的状态向量，?k,k+i是一个4X4 转移矩阵：
[0117]
[0118]式中At是两个时刻的时间差。cok是协方差矩阵的处理噪声，定义如 下：
[0119] (25)
[0120] 式中?^和?#是在k时刻向东和向北的速度噪声，按随机游走建模。
[0121]
(26)
[0122] 式中和％η是在k-Ι时刻向东和向北的速度噪声，~和nn是两个高斯白噪声，At是两个时刻的时间差。
[0123] CFP解用于平滑KF的观测量，测量模型定义如下：
[0124] zk = HkXk|k+Uk (27)
[0125] 式中zk=[rcFP(l)rcFP(2)]T采用CFP解的二维坐标作为测量向量。Hk是决策矩阵， 定义如下。
[0126]
(28)
[0127]Uk是测量噪声以高斯白噪声为模型，其协方差矩阵为R,, = ￡(% ）。KF处理过程有 两个阶段:预测和更新。在预测过程，根据系统模型预测出状态向量和协方差矩阵。 障]
(29)
[0129] 在更新过程中，根据测量模型更新状态向量和协方差矩阵：
[0130]
(30)
[0131] 式中Kk称为卡尔曼增益。
[0132] b.AKF滤波器
[0133] KF滤波器实现为平滑滤波器来平滑CFP解。在KF中为了在转弯和直行时都能工作 良好，细调过程噪声的协方差矩阵Qk就很重要。转弯时，Qk要设为较大的值以覆盖转弯的动 态变化保证KF收敛。直行时，Qk要设为较小的值以保证解的平滑。自适应设置Qk的KF通常称 为自适应KF，即AKF。AKF里Qk定义如下：
[0134] Qk = norm(vintk,Vintk-i) · Qk,s (31)
[0135] 式中Qk,s是为直行情况设置的Qk，Vintk和是组合滤波器组给出的在k时刻和k-1时刻的速度。最后，无论是实现为KF还是AKF，平滑滤波器都输出无线平滑约束指纹(SCFP) 解来融合。
[0136] 实验验证：
[0137]本发明从上述多个实施例中选定一个实施例进行实验，本实施例中，预先设定搜 索范围确定模块132中搜索范围的形状为矩形，数据匹配单元14的指纹匹配方法采用单点 确定性匹配(2-范数)用于指纹匹配，平滑滤波器32采用KF，INS/PDR/MC作为MEMS导航方案， 组合滤波器31采用EKF滤波器，用WiFi指纹作为无线指纹的代表。由一个完整的惯性单元， 三轴陀螺仪和三轴加速度计（Invensense公司MPU-6050)，和的WiFi/蓝牙组合模块(海华技 术AW-NH665)构成的待测目标来记录大量轨迹数据。
[0138]本发明采用实验1和实验2,即通过两个轨迹来将本发明中的融合导航装置所采用 的导航方法MEMS/SCFP与现有技术中的定位方法进行比较，现有技术中的MEMS独立定位和 融合定位方法如:MEMS、MEMS/FP、MEMS/CFP作比较，以及现有技术中的无线指纹识别方案： 如SCFP和FP、CFP作比较。实验环境为一 120X40平方米的室内，其中平均可见的WiFiRSS的 数目为24,在获取轨迹之前进行了WiFi无线信号地图数据库建设和维护。
[0139]如图2所示，实验1中的步行轨迹形状为"8"字形。实验1的定位结果如图3和图4所 示，图3展示了FP，CFP和SCFP的估计轨迹和定位误差。图3(a)、（b)和（c)展示了在轨迹I中 CFP和SCFP比FP的跳跃点要少。如图3 (d)所示，SCFP的定位误差比CFP稍好，比FP要好得多。 从图3可以看出本发明所公开的WiFi定位方法SCFP比其他方法更好。图4展示了MEMS、MEMS/ FP、MEMS/CFP和MEMS/SCFP的定位轨迹和定位误差。在图4 (a)、（b)、（c)和(d)中，MEMS/CFP和 MEMS/SCFP的轨迹比MEMS和MEMS/FP更接近参考值。它也表明，MEMS具有时间漂移，定位性能 最糟糕。图4(e)描述了MEMS/SCFP在所有的方法中具有最好的性能。在无线定位的帮助下， 该融合方案减少MEMS导航解的漂移。结合图3和图4,可以看出与其他定位方案相比较融合 方案更平滑并且具有更高的采样率。图5展示了轨迹I的定位误差累积分布函数(CDF)。从图 5可以看出本发明公开的MEMS/SCFP提供了比MEMS/FP或MEMS/CFP具有更好的结果。并且， SCFP的结果比CFP和FP更准确。
[0140] 如图6所示，实验2中的步行轨迹形状为"s"字形。实验2的定位结果如图7和图8所 示，图7展示了FP、CFP和SCFP的估计轨迹和定位误差，与轨迹I相似，图7(a)、（b)和（c)展示 了在轨迹Π中CFP和SCFP比FP的跳跃点要少；如图7(d)展示了在轨迹Π中SCFP的定位误差 比CFP和PF要少，这也就说明了本发明的室内导航方法比其他方法更好。图8展示了第二条 轨迹的腿]\^、]\^]\^/^、]\^]\^/^?和]\^]\^/^??的定位结果，图8(3