[发明专利]基于卡尔曼滤波的定位方法、系统、设备及存储介质

权利要求书

1.一种基于卡尔曼滤波的定位方法，其特征在于，包括：

获取智能终端的当前时刻航向角与上一时刻航向角，并计算所述当前时刻航向角与所述上一时刻航向角的航向角差值；

根据所述航向角差值，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵；

当未接收到定位模块的定位输出时，根据所述状态向量，获取智能终端的位置；

当接收到所述定位模块的定位输出时，获取所述定位模块的定位输出，作为测量值；

根据所述测量值，通过测量更新方程更新所述状态向量和误差协方差矩阵；

根据更新后的当前时刻的状态向量，获取所述智能终端的位置。

2.如权利要求1所述的基于卡尔曼滤波的定位方法，其特征在于，所述方法还包括：

根据接收到的所述定位模块输出的初始定位坐标、预设的初始速度以及预设的初始航向角对差分方向卡尔曼滤波器进行初始化处理。

3.如权利要求1所述的基于卡尔曼滤波的定位方法，其特征在于，所述根据所述航向角差值，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵包括：

通过预设的匀速运动模型，更新差分方向卡尔曼滤波器的状态转移矩阵；

根据所述航向角差值，计算所述差分方向卡尔曼滤波器的激励矩阵以及激励输入值；

通过预设的准静态白噪声模型，更新所述差分方向卡尔曼滤波器的噪声矩阵；

根据所述状态转移矩阵、所述激励矩阵、所述激励输入值以及所述噪声矩阵，通过预测更新方程预测当前时刻的状态向量并更新误差协方差矩阵。

4.如权利要求3所述的基于卡尔曼滤波的定位方法，其特征在于，所述预设的匀速运动模型为：

其中，x_k和y_k分别表示智能终端在时刻k时水平坐标系x轴和y轴的坐标，vx_k和vy_k分别表示智能终端在时刻k时水平坐标系x和y轴的速度，dt_k为状态更新的时间间隔；

所述差分方向卡尔曼滤波器的状态转移矩阵为：

5.如权利要求4所述的基于卡尔曼滤波的定位方法，其特征在于，所述根据所述航向角差值，计算所述差分方向卡尔曼滤波器的激励矩阵以及激励输入值，具体包括：

根据所述航向角差值，确定所述差分方向卡尔曼滤波器的差分方向旋转矩阵；

通过所述差分方向旋转矩阵旋转所述状态向量中的速度矢量，并根据旋转后的速度矢量以及所述状态向量中的速度矢量，计算所述差分方向卡尔曼滤波器的激励输入值：

其中，v_k＝[vx_k vy_k]^T，表示状态向量中的速度矢量；表示旋转后的速度矢量，Rot_k表示差分方向旋转矩阵，当方位角为顺时针方向夹角时，当方位角为逆时针夹角时，

根据所述激励输入值，确定匀变速运动下所述激励输入值对所述智能终端速度和位置的改变量：

根据所述激励输入值对所述智能终端的速度和位置的改变量，确定所述差分方向卡尔曼滤波器的激励矩阵：

6.如权利要求5所述的基于卡尔曼滤波的定位方法，其特征在于，所述预测更新方程为：

其中，和为对k时刻的状态向量和误差协方差矩阵的预测，s_k-1＝[x_k-1,vx_k-1,y_k-1,vy_k-1]^T，为速度白噪声方差；

所述测量更新方程为：

其中，H表示测量矩阵，R_k表示测量误差协方差矩阵，z_k为测量向量，P_k为状态的误差协方差。