[发明专利]一种定位方法及装置

权利要求书

1.一种定位方法，其特征在于，包括：

获取移动终端在第一时刻的状态向量，作为第一状态向量，其中，所述第一时刻为：当前时刻之前、与当前时刻间隔预设时长的时刻，所述第一状态向量包括：所述移动终端在所述第一时刻的三维位置信息、三维速度信息；

获取所述第一时刻的分布方差矩阵，作为第一分布方差矩阵，其中，所述第一分布方差矩阵用于表征所述第一状态向量预估值的概率分布；

获取所述移动终端接收到的毫米波通信信号在多通信路径上的到达时间；

获取所述移动终端接收到的毫米波通信信号在多通信路径上的到达角；

基于所述第一状态向量、所述第一分布方差矩阵和所述到达时间，采用扩展卡尔曼滤波算法，定位所述移动终端在当前时刻的三维位置；

所述基于所述第一状态向量、所述第一分布方差矩阵和所述到达时间，采用扩展卡尔曼滤波算法，定位所述移动终端在当前时刻的三维位置，包括：

基于所述第一状态向量、所述第一分布方差矩阵、所述到达时间和所述到达角，采用扩展卡尔曼滤波算法，定位所述移动终端在当前时刻的三维位置；

所述获取所述移动终端接收到的毫米波通信信号在多通信路径上的到达角，包括：

采用多重信号分类算法或ESPRIT算法，获取所述移动终端接收到的毫米波通信信号在多通信路径上的到达角的估值，记为第一估值；

采用自适应滤波算法，获取所述移动终端接收到的毫米波通信信号在多通信路径上的到达角的估值，记为第二估值；

计算所述第一估值和所述第二估值的平均数，得到所述移动终端接收到的毫米波通信信号在多通信路径上的到达角。

2.根据权利要求1所述的方法，其特征在于，所述基于所述第一状态向量、所述第一分布方差矩阵、所述到达时间和所述到达角，采用扩展卡尔曼滤波算法，定位所述移动终端在当前时刻的三维位置，包括：

基于所述到达角和所述移动终端的方向姿态，确定毫米波传播方向；

基于所述到达时间、所述毫米波传播方向，建立满足以下表达式的多路径测量值矩阵：

其中，k表示当前时刻，Y(k)为所述多路径测量值矩阵，g为建立所述多路径测量值矩阵所选取的通信路径的数量，θ_LOS(k)为毫米波通信信号在直射路径上的方位角，为毫米波通信信号在直射路径上的仰角，T_LOS(k)为毫米波通信信号在直射路径上的到达时间，θ_NLOS1(k)为毫米波通信信号在第一一次反射路径上的方位角，为毫米波通信信号在第一一次反射路径上的仰角，T_NLOS1(k)为毫米波通信信号在第一一次反射路径上的到达时间，θ_NLOS2(k)为毫米波通信信号在第二一次反射路径上的方位角，为毫米波通信信号在第二一次反射路径上的仰角，T_NLOS2(k)为毫米波通信信号在第二一次反射路径上的到达时间，θ_NLOS(g-1)(k)为毫米波通信信号在第(g-1)一次反射路径上的方位角，为毫米波通信信号在第(g-1)一次反射路径上的仰角，T_NLOS(g-1)(k)为毫米波通信信号在第(g-1)一次反射路径上的到达时间，其中，方位角为毫米波传播方向在水平面内的投影的角度，仰角为毫米波传播方向与水平面的夹角；

采用以下公式，获得当前时刻的状态向量的预估状态向量：

其中，k-1表示所述第一时刻，为所述预估状态向量，s(k-1)为所述第一状态向量，F为状态转移矩阵，I为单位矩阵，Δt为所述预设时长；

采用以下公式，获得当前时刻的分布方差矩阵的预估分布方差矩阵：

其中，为所述预估分布方差矩阵，P(k-1)为所述第一分布方差矩阵；

在所述预估状态向量所在的向量空间，基于所述预估状态向量和所述预估分布方差矩阵，生成2n+1个抽样点，其中，n为所述预估状态向量的维度；

通过预设的表征状态向量与多路径测量值矩阵之间关系的函数，将2n+1个所述抽样点转化为2n+1个抽样矩阵；

基于所述抽样矩阵和所述抽样点的统计量，获得所述预估状态向量的修正值；

将所述预估状态向量与所述预估状态向量的修正值相加，得到当前时刻的状态向量；

根据所述当前时刻的状态向量，定位所述移动终端在当前时刻的三维位置。