[发明专利]一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法

权利要求书

1.一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，包括如下步骤：

S1，在待测室内区域内，随机部署zigbee信号强度测量网络；

S2，采用上位机获得路径上各点接收到的RSSI并计算出对应距离d；与此同时采用地磁测量模块沿着相同的路径依次测得路径上各点的地磁数据；

S3，采用三边定位法依据所述上位机计算出的对应距离求解路径路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径；与此同时，采用基于Hausdorff的距离度量算法进行地磁数据的搜索匹配，并对轨迹进行跟踪以获得路径；

S4，采用适宜该环境的加权系数将所述S3步骤中获得的两个路径加和，以输出最终路径，该最终路径即为当前位置至定位位置的路径。

2.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S1步骤具体为：在a(m)×a(m)待测的室内区域内，随机部署n个传感器，传感器的节点坐标为：(x_i,y_i)，其中i＝1,2,...,n.n≥3。

3.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S2步骤中，计算出对应距离d具体为：

协调器节点坐标为(m,n)，终端节点与协调器之间采用ZigBee自组网方式进行无线数据传输，协调器与上位机使用串口进行通信，并通过上位机获得协调器接收到的终端节点发射出来的无线信号强度，计算该环境下的环境参数A，n，利用所得的信号强度通过公式RSSI＝-(10nlgd+A)计算出对应距离d。

4.如权利要求3所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S3步骤中：

采用三边定位法求解路径上各点位置的测量坐标，并用卡尔曼滤波算法对轨迹进行跟踪定位以获得路径具体为：

S300，带入所述S2步骤中求得的对应距离d，采用三边算法求解路径上各点位置：

设定均匀部署在室内区域中的传感器节数为三个，三个传感器所在的参考节点的位置坐标分别为A(x₁,y₁)、B(x₂,y₂)、C(x₃,y₃)，协调器的位置为待确定节点M，其坐标为M(m,n)，M到三个参考节点的距离分别是d₁、d₂和d₃，如下式：

由上式得出节点M的位置坐标为

S301，采用卡尔曼滤波模拟运动轨迹：

将所述S300步骤测得轨迹上每个点的测量坐标依次代入卡尔曼滤波模型中进行滤波，卡尔曼滤波的递推思想按前一时刻的最佳估计值来预测当前时刻的状态值，然后通过当前时刻的观测值来修正该状态值，得到当前时刻的最佳估计值，以此递推下去，在这个过程中，主要包括预测和更新两部分：

其中第一个公式为状态预测部分公式，第二个公式为状态更新部分公式，其中x₀和∑₀为初始状态均值和方差，K_t为卡尔曼滤波增益矩阵，∑_t为增益矩阵协方差，为前一时刻的最佳估计值，为当前时刻的状态预测值，为当前时刻的最佳估计值，y_t为当前时刻的观测值。

5.如权利要求1所述的一种基于地磁强度的卡尔曼滤波室内轨迹跟踪算法，其特征在于，在所述S2步骤中，地磁数据的采集分为两部分：地磁数据库的建立与测量轨迹地磁数据获取。