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

权利要求书

1.一种基于非线性滤波的磁定位方法，其特征在于：采用磁传感器观测网实现磁性目标的精确定位跟踪，包括如下步骤：

S1、读取磁传感器观测网中各磁传感器在参考直角坐标系中的位置，以周期同步采样的方式读取磁性目标内置永磁体在各磁传感器处的磁通量密度测量值；

S2、以磁性目标的位置和方向参数为状态变量，采用当前统计模型建立状态方程；以各磁传感器测得的磁通量密度为测量输出，根据毕奥-萨伐尔定律计算的磁通量密度值为理论值，建立量测方程。

2.根据权利要求1所述的一种基于非线性滤波的磁定位方法，其特征在于状态空间方程的建立包含以下步骤：

S201、预设磁性目标加速度极值，机动时间常数，采样频率；

S202、由下式的当前统计模型对磁性目标建立状态方程：

其中F_k是状态转移矩阵；ψ_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时刻的离散时间白噪声；

S203、由下式建立磁性目标的量测方程

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

B′_l,k+1＝[B'_lx,k+1 B'_ly,k+1 B'_lz,k+1]^T (3)

其中B'_lx,k+1，B'_ly,k+1和B'_lz,k+1是第l个磁传感器测量的在参考直角坐标系x，y，z方向的磁通量密度值，l＝1,2,…,N，N为磁传感器的个数；V_l,k+1是第l个磁传感器k+1时刻的离散时间白噪声；h_l(ψ_k+1)是第l个磁传感器根据毕奥-萨伐尔定律计算得到的ψ_k+1处磁性目标在参考直角坐标系x，y，z方向的磁通量密度理论值，并且有：

其中h_l(ψ_k+1)是k+1时刻第l个磁传感器根据毕奥-萨伐尔定律计算的在参考直角坐标系x，y，z方向磁通量密度理论值；x_l,y_l,z_l是第l个磁传感器在参考直角坐标系下的x，y，z坐标；R_l,k+1是第l个磁传感器到磁性目标的距离；μ是磁导率，M是磁化强度。

3.根据权利要求2所述的一种基于非线性滤波的磁定位方法，其特征在于：对于由(1)式和(2)式所构成的表示磁性目标定位的非线性系统状态空间模型，采用平方根无迹卡尔曼滤波算法估计得到磁性目标的位置和方向。