CN110763238A - 基于uwb、光流和惯性导航的高精度室内三维定位方法 - Google Patents

Abstract

本发明涉及室内定位技术领域，特别涉及一种基于UWB、光流和惯性导航的高精度室内三维定位方法，包括对加速度计和陀螺仪进行误差补偿，完成初始对准；进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息并与超宽带UWB定位信息、光流传感器的速度信息构建量测矩阵；构建载体的室内三维定位的状态方程，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值，该值即为载体的室内三维定位信息；本发明能实现全自主室内导航，比传蓝牙信标室内定位所需布设基站可以大幅度减少，比单纯UWB室内定位能大幅度减少基站布设以及定位更连续。

Description

基于UWB、光流和惯性导航的高精度室内三维定位方法

技术领域

本发明涉及室内定位技术领域，特别涉及一种基于UWB、光流和惯性导航的高精度室内三维定位方法。

背景技术

目前的室内定位技术方面的技术，有基于UWB(Ultra-Wideband，UWB)超宽带，iBeacon蓝牙信标，RFID(Radio Frequency Identification)射频识别，WIFI无线网络等技术的室内定位技术比较成熟。

目前主流的室内定位技术，均需要大量的在室内布置信标，这个工作量很大，且硬件成本和施工成本都比较高，在无法接收信号的地方无法实现室内有效定位。

由于UWB信号的高时间分辨率，TOA(Time of Arriva,到达时间定位)和TDOA(TimeDifference of Arriva,到达时间差定位)相对于其他算法具有更高的准确度。对目前UWB定位来说最有效的解决方案是采用TOA与TDOA的混合定位算法，因为结合了两种算法的优点。TOA技术是指由基站向移动站发出特定的测距命令或指令信号，并要求终端对该指令进行响应。基站会纪录下由发出测距指令到收到终端确认信号所花费的时间，该时间主要由射频信号在环路上的传播时延、终端的响应时延和处理时延、基站的处理时延组成。如果能够准确地得到终端和基站的响应和处理时延，就可以算出射频信号的环路传播时延。因为无线电波在空气中以光速传播，所以基站与终端之间的距离可以估算出来。当有三个基站参与测量时，就可以根据三角定位法来确定终端所在的区域。TDOA定位算法是一种利用时间差进行定位的方法，通过测量信号达到基站的时间，可以确定信号源的距离，利用信号源到多个无线电监测站的距离(以无线电基站为中心，距离为半径作圆)，就能确定信号的位置。通过比较信号到达多个基站的时间差，就能做出以检测站为焦点、距离差为长轴的双曲线的交点，该交点即为信号的位置。

光流传感器通过图像采集***以一定速率连续采集物体表面图像，再由DSP对所产生的图像数字矩阵进行分析。由于相邻的两幅图像总会存在相同的特征通过对比这些特征点的位置变化信息，便可以判断出物体表面特征的平均运动，这个分析结果最终被转换为二维的坐标偏移量，并以像素数形式存储在特定的寄存器中，实现对运动物体的检测。

发明内容

为了实现全自主室内导航，本发明提出一种基于UWB、光流和惯性导航的高精度室内三维定位方法，载体设置有惯性导航仪、惯性传感器、光流传感器、磁传感器以及超宽带定位，使用惯性导航仪融合光流传感器、磁传感器、超宽带定位等信息来测量载体的速度、位置、以及姿态信息；具体的室内三维定位过程包括：

惯性传感器包括加速度计和陀螺仪，对加速度计和陀螺仪进行误差补偿，完成初始对准；

进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息；

将载体的实时速度、位置、以及姿态信息与超宽带UWB定位信息、光流传感器的速度信息构建矩阵；

构建载体的室内三维定位的状态方程，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值，该值即为载体的室内三维定位信息。

进后验的，量测向量表示为：

其中，Z为量测向量；V_GE为光流传感器测得的东向速度；V_IE为惯性传感器测得的东向速度；V_GN为光流传感器测得的北向速度；V_IN为惯性传感器测得的北向速度；L_G为UWB定位得到的纬度值；L_I为惯性传感器计算得到的纬度值；λ_G为UWB定位得到的经度值；λ_I为惯性传感器计算得到的经度值；h_A为UWB定位得到的高度值；h_I为惯性传感器计算得到的高度值；ψ_G为磁传感器计算得到的航向角；ψ_I为惯性传感器计算得到的航向角。

进后验的，载体的室内三维定位的状态方程表示为：

其中，X_k+1表示第k+1时刻的状态矩阵；Φ_k+1,k表示状态向量均方差矩阵；W_k表示第k时刻的状态矩阵噪声向量；Z_k+1表示第k+1时刻的量测向量；H_k+1表示第k+1时刻的量测矩阵；V_k+1表示第k+1时刻的量测矩阵噪声向量。

进后验的，状态矩阵X表示为：

其中，δV_E为东向速度误差，为惯性传感器测得的东向速度与光流传感器测得的东向速度的差值；δV_N北向速度误差，为惯性传感器测得的北向速度与光流传感器测得的北向速度的差值；δV_U为天向速度误差，为惯性传感器测得的天向速度与光流传感器测得的天向速度的差值；δL为纬度误差，为惯性导航仪测量的纬度值与UWB定位得到的纬度值的差；δλ为经度误差，为惯性导航仪测量的经度值与UWB定位得到的经度值的差；δh为高度误差，为惯性导航仪测量的高度值与UWB定位得到的高度值的差；

为东向失准角误差，为惯性导航仪测得的东向失准角与陀螺仪测量并进行补偿之后的东向失准角的差值；

为北向失准角误差，为惯性导航仪测得的北向失准角与陀螺仪测量并进行补偿之后的北向失准角的差值；

为天向失准角误差，为惯性导航仪测得的天向失准角与陀螺仪测量并进行补偿之后的天向失准角的差值；ε_x为x轴陀螺零漂；ε_y为y轴陀螺零漂；ε_z为z轴陀螺零漂；

为x轴加速度计零漂；

为y轴加速度计零漂；

为z轴加速度计零漂。

进后验的，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值的过程包括：

根据第k时刻的状态矩阵预测第k+1时刻的状态矩阵，获得第k+1时刻的后验预测状态矩阵：

更新第k+1时刻的后验预测状态矩阵的状态，获得第k+1时刻的预测状态矩阵：

第k+1时刻的卡尔曼滤波增益K_k+1为：

根据第k时刻的均方误差预测第k+1时刻的均方误差，获得第k+1时刻的后验预测均方误差：

更新第k+1时刻的后验预测均方误差的状态，获得第k+1时刻的预测均方误差：

其中，

为第k+1时刻的后验预测状态向量；

为第k时刻的状态向量；Φ_k+1,k为状态向量的均方差矩阵；Z_k+1为第k+1时刻的量测向量；H_k+1为第k+1时刻的量测矩阵；P_k+1/k为第k+1时刻的均方误差矩阵；P_k为第k时刻的均方误差矩阵；Q_k为状态参数的噪声矩阵；I为单位阵；R_k+1为第k+1时刻的观测噪声矩阵；上标T表示矩阵的转置；上标-1表示矩阵的逆矩阵。

本发明的有益效果在于：

1.本发明提出的基于UWB、光流和惯性导航的高精度室内三维定位，不依赖于外部信号，能实现全自主室内导航，比传蓝牙信标室内定位所需布设基站可以大幅度减少，比单纯UWB室内定位能大幅度减少基站布设以及定位更连续；

2.本发明提出的基于UWB、光流和惯性导航的高精度室内三维定位方法与传统的是室内方法相比较，具有更高的定位精度。

附图说明

图1为本发明根据定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度的示意图；

图2为本发明根据定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度的示意图；

图3为本发明基于UWB、光流和惯性导航的高精度室内三维定位方法的原理示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提出一种基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，载体设置有惯性导航仪、惯性传感器、光流传感器、磁传感器以及超宽带定位，使用惯性导航仪融合光流传感器、磁传感器、超宽带定位等信息来测量载体的速度、位置、以及姿态信息；具体的室内三维定位过程包括：

惯性传感器包括加速度计和陀螺仪，对加速度计和陀螺仪进行误差补偿，完成初始对准；

进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息；

将载体的实时速度、位置、以及姿态信息与超宽带UWB定位信息、光流传感器的速度信息构建量测方程和量测向量；

对实时的量测矩阵进行卡尔曼滤波，设定状态矩阵X、状态向量初始均方差矩阵P_k，状态向量均方差矩阵Φ_k+1,k、观测均方差矩阵H_k+1的初始值，其中状态矩阵的X初始值均为0，状态向量初始均方差矩阵P_k、状态向量均方差矩阵Φ_k+1,k和观测均方差矩阵H_k+1初始值根据惯性传感器、光流传感器、磁传感器以及超宽带定位采集数据计算获得；

获取根据初始的状态矩阵和均方误差或者上一个时刻的状态矩阵和均方误差获取实时的量测矩阵；根据实时的量测矩阵获得载体实时的室内三维定位的状态矩阵，即获得载体的室内三维定位信息。

在本实施例中，构建的量测向量表示为：

其中，Z为量测向量；V_GE为光流传感器测得的东向速度；V_IE为惯性传感器测得的东向速度；V_GN为光流传感器测得的北向速度；V_IN为惯性传感器测得的北向速度；L_G为UWB定位得到的纬度值；L_I为惯性传感器计算得到的纬度值；λ_G为UWB定位得到的经度值；λ_I为惯性传感器计算得到的经度值；h_A为UWB定位得到的高度值；h_I为惯性传感器计算得到的高度值；ψ_G为磁传感器计算得到的航向角；ψ_I为惯性传感器计算得到的航向角。

作为一种可选的实施方式，采用到达时间定位(Time of Arriva,TOA)算法利用超宽带定位基站计算定位位置O的坐标，如图1，具体包括：

已知3个定位基站R1、R2以及R3 R4的坐标，分别为：R1(x1,y1,z1)、R2(x2,y2,z2)以及R3(x3,y3,z3)，若脉冲信号从定位位置O到R1、R2以及R3这3个基站的时间分别为为t1、t2、t3，根据脉冲信号的传播速度以及脉冲信号从定位位置O到R1、R2以及R3的时间计算出3个定位基站与定位位置O的相对距离，每个基站以相对距离为半径画一个球形轨迹，利用三个球形方程能够计算出唯一的交点，该交点为定位位置O的坐标，计算过程包括：

其中，v为脉冲信号传播的速度。

作为另一种可选的实施方式，采用到达时间差定位(Time Difference ofArriva,TDOA)算法利用超宽带定位基站计算定位位置O的坐标，TDOA定位是一种利用时间差进行计算的方法。精准的绝对时间相对较难测量，通过比较信号到达各个UWB定位基站的时间差，计算出信号到各个定位基站的距离差，就能作出以定位基站为焦点，距离差为长轴的双曲线，三组双曲线的交点就是定位标签的位置，不同于TOA的是，TDOA是通过检测信号到达两个基站的时间差，而不是到达的绝对时间来确定移动台的位置，因此降低了***对时间同步的要求，TDOA算法是对TOA算法的改进，与TOA算法相比，不需要加入专门的时间戳，定位精度也有所提高；如图2，采用TDOA算法利用超宽带定位基站计算定位位置O的坐标具体包括：

已知4个定位基站R1、R2、R3以及R4的坐标，分别为：R1(x1,y1,z1)、R2(x2,y2,z2)、R3(x3,y3,z3)以及R4(x4,y4,z4)，若脉冲信号从定位位置O到R1、R2、R3以及R4四个基站的时间分别为为t1、t2、t3以及t4，分别以(R1、R4)，(R2、R4)，(R3、R4)做为焦点定位位置O，发送的信号到两基站间的距离差为常数，可以得到3组双曲线，双曲线的交点即是定位标签O(x0,y0,z0)的坐标，根据其标签O(x0,y0,z0)的坐标确定定位位置O的经纬度以及高度，求解定位标签O的坐标的方程表示为：

利用光流传感器计算运动物体的速度的过程中，首先通过图像平面三维速度场的投影构建运动场，若三维相机参考帧中的一个点P的空间坐标表示为：P＝[X,Y,Z]^T,其中光学轴是这帧的Z轴，f表示焦距，投影中心在原点，则图像平面上P的像素坐标的投影表示为：

由于焦距f等于图像平面到原点的距离，投影p表示为p＝[x,y,f]^T，相机和P点之间的相对运动表示为：

V＝-T-ωP；

其中，ω是运动的角速率，T是运动的平动分量，对图像平面上P的像素坐标的投影两边的时间进行求导，得到相机参考帧中P的速度与图像平面中p的速度或光流之间的关系为：

用x和y分量表示，代入相机和P点之间的相对运动，得到：

运动场分量等于纯平移分量加上纯旋转分量。旋转部分不依赖于Z，因此角速度不携带场景深度信息。

v_x、v_y的平移分量根据焦距和当前到场景的距离z进行缩放，如果需要平动速度，且如果旋转矢量为零或已知(由陀螺仪测量)，并由运动场进行补偿，则可以通过下式给出：

运动场的平移部分与场景的距离测量相结合，如果可以假定到场景的距离近似恒定，则会产生公制尺度的平移速度，尤其是当相机垂直地面时。

在本实施例中，如图3，将光流传感器采集的数据计算得到速度信息，磁传感器采集的磁通量信息，超宽带定位采集的经度、纬度和高度信息，以及惯性传感器采集并补偿之后得到的加速度和角速度信息作为卡尔曼滤波器的输入，并根据卡尔曼滤波器的增益预测载体的速度、位置、姿态失准角误差以及陀螺零漂、加速度零漂，并将该预测的误差反馈给载体的速度、位置、姿态失准角、陀螺零漂和加速度零漂，载体的速度误差、位置误差、姿态失准角误差、陀螺零漂以及加速度零漂构成载体的室内三维定位的状态矩阵；其中，速度误差为惯性导航仪计算的速度与光流传感器测量的速度的差值，速度包括东向速度、北向速度以及天向速度；位置误差为惯性导航计算的位置与UWB定位的位置的误差，位置包括经度、纬度以及高度；姿态失准角误差为惯性导航计算的姿态失准角与经过修正的惯性传感器计算的姿态失准角之间的差值，姿态失准角包括东向失准角、北向失准角以及天向失准角。

载体的室内三维定位的状态矩阵表示为：

其中，X_k+1表示第k+1时刻的组合导航的状态矩阵；Φ_k+1,k表示状态向量均方差矩阵；W_k表示第k时刻的状态矩阵噪声向量；Z_k+1表示第k+1时刻的量测向量；H_k+1表示第k+1时刻的量测矩阵；V_k+1表示第k+1时刻的量测矩阵噪声向量。

在本实施例中，状态矩阵X表示为：

其中，δV_E为东向速度误差，为惯性传感器测得的东向速度与光流传感器测得的东向速度的差值；δV_N北向速度误差，为惯性传感器测得的北向速度与光流传感器测得的北向速度的差值；δV_U为天向速度误差，为惯性传感器测得的天向速度与光流传感器测得的天向速度的差值；δL为纬度误差，为惯性导航仪测量的纬度值与UWB定位得到的纬度值的差；δλ为经度误差，为惯性导航仪测量的经度值与UWB定位得到的经度值的差；δh为高度误差，为惯性导航仪测量的高度值与UWB定位得到的高度值的差；

为东向失准角误差，为惯性导航仪测得的东向失准角与陀螺仪测量并进行补偿之后的东向失准角的差值；

为北向失准角误差，为惯性导航仪测得的北向失准角与陀螺仪测量并进行补偿之后的北向失准角的差值；

为天向失准角误差，为惯性导航仪测得的天向失准角与陀螺仪测量并进行补偿之后的天向失准角的差值；ε_x为x轴陀螺零漂；ε_y为y轴陀螺零漂；ε_z为z轴陀螺零漂；

为x轴加速度计零漂；

为y轴加速度计零漂；

为z轴加速度计零漂。

根据***的离散方程，可利用如下的卡尔曼滤波算法来求解状态变量的估计值：

根据第k时刻的状态矩阵预测第k+1时刻的状态矩阵，获得第k+1时刻的后验预测状态矩阵：

更新第k+1时刻的后验预测状态矩阵的状态,获得第k+1时刻的预测状态矩阵：

第k+1时刻的卡尔曼滤波增益K_k+1为：

根据第k时刻的均方误差预测第k+1时刻的均方误差，获得第k+1时刻的后验预测均方误差：

更新第k+1时刻的后验预测均方误差的状态,获得第k+1时刻的预测均方误差：

其中，

为第k+1时刻的后验预测状态向量；

为第k时刻的状态向量；Φ_k+1,k为状态向量的均方差矩阵；Z_k+1为第k+1时刻的量测向量；H_k+1为第k+1时刻的量测矩阵；P_k+1/k为第k+1时刻的均方误差矩阵；P_k为第k时刻的均方误差矩阵；Q_k为状态参数的噪声矩阵；I为单位阵；R_k+1为第k+1时刻的观测噪声矩阵；上标T表示矩阵的转置；上标-1表示矩阵的逆矩阵。

利用该卡尔曼滤波的算法，只要给定初值

和P_k，根据不同时刻所得到的测量Z_k，就可得到相应时刻的状态估计

通过实验得到，基于UWB、光流和惯性导航的高精度室内三维定位方法所需基站数要比传统的室内定位方法少，原因在于惯性导航、UWB和光流的信息融合作用，并使得定位精度更高更连续。

基于UWB、光流和惯性导航的高精度室内三维定位方法与传统的室内定位方法性能对比如表1，本发明的方法与基于蓝牙信标的室内三维定位方法和基于UWB的室内三维定位方法的方法相比，在相同的区域范围内需要的基站数量更少，定位精度更精确。

表1

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (8)

1.基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，载体设置有惯性导航仪、惯性传感器、光流传感器、磁传感器以及超宽带定位，使用惯性导航仪融合光流传感器、磁传感器、超宽带定位的信息来测量载体的速度、位置、以及姿态信息；具体的室内三维定位过程包括：

惯性传感器包括加速度计和陀螺仪，对加速度计和陀螺仪进行误差补偿，完成初始对准；

进入导航阶段，将加速度计和陀螺仪实时测量的参数进行导航解算，获得载体的实时速度、位置、以及姿态信息；

将载体的实时速度、位置、以及姿态信息与超宽带UWB定位信息、光流传感器的速度信息构建量测矩阵；

构建载体的室内三维定位的状态方程，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值，该值即为载体的室内三维定位信息。

2.根据权利要求1所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，量测向量表示为：

其中，Z为量测向量；V_GE为光流传感器测得的东向速度；V_IE为惯性传感器测得的东向速度；V_GN为光流传感器测得的北向速度；V_IN为惯性传感器测得的北向速度；L_G为UWB定位得到的纬度值；L_I为惯性传感器计算得到的纬度值；λ_G为UWB定位得到的经度值；λ_I为惯性传感器计算得到的经度值；h_A为UWB定位得到的高度值；h_I为惯性传感器计算得到的高度值；ψ_G为磁传感器计算得到的航向角；ψ_I为惯性传感器计算得到的航向角。

3.根据权利要求2所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度包括：已知3个定位基站R1、R2以及R3 R4的坐标，分别为：R1(x1,y1,z1)、R2(x2,y2,z2)以及R3(x3,y3,z3)，若脉冲信号从定位位置O到R1、R2以及R3这3个基站的时间分别为为t1、t2、t3，根据脉冲信号的传播速度以及脉冲信号从定位位置O到R1、R2以及R3的时间计算出3个定位基站与定位位置O的相对距离，每个基站以相对距离为半径画一个球形轨迹，利用三个球形方程能够计算出唯一的交点，该交点为定位位置O的坐标，计算过程包括：

其中，v为脉冲信号传播的速度。

4.根据权利要求2所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，定位基站根据到达时间定位TOA算法确认定位位置的经纬度以及高度包括：已知4个定位基站R1、R2、R3以及R4的坐标，分别为：R1(x1,y1,z1)、R2(x2,y2,z2)、R3(x3,y3,z3)以及R4(x4,y4,z4)，若脉冲信号从定位位置O到R1、R2、R3以及R4四个基站的时间分别为为t1、t2、t3以及t4，分别以(R1、R4)，(R2、R4)，(R3、R4)做为焦点定位位置O，发送的信号到两基站间的距离差为常数，可以得到3组双曲线，双曲线的交点即是定位标签O(x0,y0,z0)的坐标，根据其标签O(x0,y0,z0)的坐标确定定位位置O的经纬度以及高度，求解定位标签O的坐标的方程表示为：

其中，v为脉冲信号传播的速度。

5.根据权利要求2所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，光流传感器测得的东向速度以及北向速度的过程包括：由图像平面三维速度场的投影建立运动场，将相机参考帧中一个点P的像素坐标的投影在原点，获得投影p，将点P处的速度与图像平面中p的速度或光流之间的关系代入相机和P点之间的运动场，可以得出：

其中，点P表示为P＝[X,Y,Z]^T，X为点P的经度，Y为点P的纬度,为点P的Z高度；投影p表示为p＝[x,y,z]^T，x为投影p的经度，y为投影p的纬度，z为投影p的相机焦距,即图像平面到原点的距离等于焦距f，z＝f；v_x表示东向运动速度；v_y表示北向运动速度；T_z表示z轴上的运动的平动分量；T_x表示x轴上的运动的平动分量；T_y表示y轴上的运动的平动分量；ω_x表示y轴上运动的角速度；ω_y表示y轴上运动的角速度；ω_z表示z轴上运动的角速度。

6.根据权利要求1所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，载体的室内三维定位的状态方程表示为：

其中，X_k+1表示第k+1时刻的状态矩阵；Φ_k+1,k表示状态向量均方差矩阵；W_k表示第k时刻的状态矩阵噪声向量；Z_k+1表示第k+1时刻的量测向量；H_k+1表示第k+1时刻的量测矩阵；V_k+1表示第k+1时刻的量测矩阵噪声向量。

7.根据权利要求1所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，状态矩阵X表示为：

其中，δV_E为东向速度误差，为惯性传感器测得的东向速度与光流传感器测得的东向速度的差值；δV_N北向速度误差，为惯性传感器测得的北向速度与光流传感器测得的北向速度的差值；δV_U为天向速度误差，为惯性传感器测得的天向速度与光流传感器测得的天向速度的差值；δL为纬度误差，为惯性导航仪测量的纬度值与UWB定位得到的纬度值的差；δλ为经度误差，为惯性导航仪测量的经度值与UWB定位得到的经度值的差；δh为高度误差，为惯性导航仪测量的高度值与UWB定位得到的高度值的差；

为东向失准角误差，为惯性导航仪测得的东向失准角与陀螺仪测量并进行补偿之后的东向失准角的差值；

为北向失准角误差，为惯性导航仪测得的北向失准角与陀螺仪测量并进行补偿之后的北向失准角的差值；

为天向失准角误差，为惯性导航仪测得的天向失准角与陀螺仪测量并进行补偿之后的天向失准角的差值；ε_x为x轴陀螺零漂；ε_y为y轴陀螺零漂；ε_z为z轴陀螺零漂；为x轴加速度计零漂；为y轴加速度计零漂；

为z轴加速度计零漂。

8.根据权利要求1所述的基于UWB、光流和惯性导航的高精度室内三维定位方法，其特征在于，根据卡尔曼滤波算法以及不同时刻的量测矩阵，求解状态方程获得不同时刻的状态矩阵的估计值的过程包括：

根据第k时刻的状态矩阵预测第k+1时刻的状态矩阵，获得第k+1时刻的后验预测状态矩阵：

更新第k+1时刻的后验预测状态矩阵的状态，获得第k+1时刻的预测状态矩阵：

第k+1时刻的卡尔曼滤波增益K_k+1为：

根据第k时刻的均方误差预测第k+1时刻的均方误差，获得第k+1时刻的后验预测均方误差：

更新第k+1时刻的后验预测均方误差的状态，获得第k+1时刻的预测均方误差：

其中，

为第k+1时刻的后验预测状态向量；

为第k时刻的状态向量；Φ_k+1,k为状态向量的均方差矩阵；Z_k+1为第k+1时刻的量测向量；H_k+1为第k+1时刻的量测矩阵；P_k+1/k为第k+1时刻的均方误差矩阵；P_k为第k时刻的均方误差矩阵；Q_k为状态参数的噪声矩阵；I为单位阵；R_k+1为第k+1时刻的观测噪声矩阵；上标T表示矩阵的转置；上标-1表示矩阵的逆矩阵。

Images