[发明专利]一种基于全向搜索的位置滤波方法

权利要求书

1.一种基于全向搜索的位置滤波方法，其特征在于，其包括：

每个处理周期产生目标位置实测信息，由实测信息计算得到实测矩阵；

根据所述预测计算方法和预测转换矩阵，由实测矩阵得到预测矩阵；

根据所述量测计算方法和量测转换矩阵，由预测矩阵得到量测矩阵；

根据所述新息计算方法和新息转换矩阵，由量测矩阵得到新息矩阵；

根据所述滤波计算方法和滤波转换矩阵，有新息矩阵得到滤波矩阵；

计算目标的位置滤波矩阵，得到下一周期目标位置滤波信息，包括：目标滤波距离r_f，目标滤波方位角fy_f，目标滤波俯仰角fz_f。

2.根据权利要求1所述的基于全向搜索的位置滤波方法，其特征在于，所述n时刻目标位置实测信息包括：目标实测距离r_n，目标实测方位角fy_n，目标实测俯仰角fz_n；计算得到n时刻实测矩阵，实测矩阵H_t(n)表示为：

H_t(n)[(ax_n,ay_n,az_n)；(vx_n,vy_n,vz_n)]

其中，ax_n表示n时刻目标位置x坐标，ay_n表示n时刻目标位置y坐标，az_n表示n时刻目标位置z坐标，vx_n表示n时刻目标速度x坐标，vy_n表示n时刻目标速度y坐标，vz_n表示n时刻目标速度z坐标；

ax_n＝r_n*cos(fy_n)*cos(fz_n)

ay_n＝r_n*sin(fz_n)

az_n＝r_n*sin(fy_n)*cos(fz_n)

其中，r_n表示n时刻目标实测距离，fy_n表示n时刻目标实测方位角，fz_n表示n时刻目标实测俯仰角

其中，Δt表示周期时间间隔。

3.根据权利要求1所述的基于全向搜索的位置滤波方法，其特征在于，所述由目标实测矩阵经过预测计算方法得到目标n+1时刻的预测矩阵，预测矩阵H_p(n+1)表示为：

H_p(n+1)[(ax_n+1,ay_n+1,az_n+1)；(vx_n+1,vy_n+1,vz_n+1)]＝P*H_t(n)

其中，ax_n+1表示n+1时刻目标预测位置x坐标，ay_n+1表示n时刻目标预测位置y坐标，az_n+1表示n+1时刻目标预测位置z坐标，vx_n+1表示n+1时刻目标预测速度x坐标，vy_n+1表示n+1时刻目标预测速度y坐标，vz_n+1表示n+1时刻目标预测速度z坐标，H_t(n)表示n时刻的实测矩阵，P表示预测转换矩阵；

其中，Δt为周期时间间隔。

4.根据权利要求1所述的基于全向搜索的位置滤波方法，其特征在于，所述由目标预测矩阵经过量测计算方法得到目标n+1时刻的量测矩阵，量测矩阵Z表示为：

Z＝H_Z*H_t(n)

其中，H_t(n)表示n时刻的实测矩阵；H_Z表示量测转换矩阵；