[发明专利]一种基于非线性滤波的磁定位方法

说明书

本发明公开了一种基于非线性滤波的磁定位方法，属于磁定位技术领域。所述定位方法包括以下步骤：A.设置磁传感器观测网，获得所有磁传感器的空间坐标；B.通过各个磁传感器测量得到磁性目标内置永磁体的磁通量密度，用当前统计模型建立以磁性目标位置和方向为变量的状态方程，用毕奥‑萨伐尔定律和磁传感器的测量值建立量测方程；C.用平方根无迹卡尔曼滤波算法估计磁性目标的位置和方向，以连续周期同步采样‑状态估计的方式实现对磁性目标的精确定位跟踪。

技术领域

本发明涉及一种基于非线性滤波的磁定位方法，属于磁定位技术领域。

背景技术

近年来，无线定位技术已经得到了广泛的应用。近距离，尤其是在非视距场景下的高精度、低成本的定位技术有极大的应用价值。胶囊内窥镜对于胃肠道病变的检查因其具有无创伤等优点而得到广泛应用。其准确实时定位可以帮助医生快速获取病灶部位的准确信息，以辅助医生对患者进行诊断和治疗。

目前，现有技术之一，电磁定位在基于到达时间(ToA)、接收信号强度(RSS)、到达时间差(TDoA)等技术方法方面的难度主要在于：人体的结构复杂，不同器官具有差异较大的介电常数，从而对电磁波的传播速度等有较大影响，使得准确的信号模型建立比较困难，所以这方面目前通常定位精度较低。

现有技术之二，基于磁定位的技术。通常是通过Levenberg-Marquardt(LM)算法，或者粒子群优化等算法求解非线性误差函数得到磁性目标的位置和方向。由于在计算过程中存在迭代，使得实时性较差，同时抗传感器量测噪声性能较差。

发明内容

本发明提供一种基于非线性滤波的磁定位技术，实现实时、连续和高精度地定位跟踪磁性目标，同时具有良好的抗传感器量测噪声性能。

为了达到上述目的，本发明提供一种基于非线性滤波的磁定位技术，包含以下步骤：

步骤S1、用下式表示的当前统计模型对磁性目标建立状态方程：

其中ψ_k+1和ψ_k分别表示磁性目标k+1和k时刻状态向量，ψ_k＝[a_k,b_k,c_k,m_k,n_k,p_k]^T，[a_k,b_k,c_k]^T表示磁性目标k时刻的位置，[m_k,n_k,p_k]^T表示磁性目标k时刻的方向(这里的位置和方向指可磁偶极子模型化永磁体的位置和方向，下同)；G_k表示磁性目标k时刻的控制矩阵；表示磁性目标机动加速度平均值；w_k表示k时刻的离散时间白噪声。

w_k的方差为为实对称阵。

其中T为采样周期；α为自相关时间常数，与T有关；

步骤S2、测量方程由下式表示：

B'_l,k+1＝h_l(ψ_k+1)+V_l,k+1