一种车辆航迹估计方法、装置、设备及介质

技术领域

本发明涉及智能驾驶技术领域，特别涉及一种车辆航迹估计方法、装置、设备及介质。

背景技术

在自动驾驶(Auto Drive，即AD)、高级辅助驾驶(Advanced Driving AssistanceSystem，即ADAS)领域，依托车身配备的传感器，比如轮速传感器、惯性传感器(InertialMeasurement Unit，即IMU)生成的包含速度、角速度以及航迹估计的里程计，被广泛的应用于后端模块的组合、优化和决断。

然而，随着使用场景的不断扩展，对里程计的性能要求也越来越高，比如要适应于立体三维航迹估计，要有更高精度的速度、角速度。也就是说，现有的车辆航迹估计方法依赖较高性能的惯性传感器，较难适用于立体航迹的高精度推算或估计，且稳定性和精度都仍待提高。

综上，如何实现稳定、精准的车辆三维航迹估计是目前有待解决的问题。

发明内容

有鉴于此，本发明的目的在于提供一种车辆航迹估计方法、装置、设备及介质，能够实现稳定、精准的车辆三维航迹估计。其具体方案如下：

第一方面，本申请公开了一种车辆航迹估计方法，包括：

确定当前目标车辆对应的当前观测向量；

将所述当前观测向量输入至目标滤波器，以便基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵；其中，所述目标滤波器中设置有预设滤波器更新方法；

基于所述当前估计向量得到所述当前目标车辆的航迹信息；

其中，观测向量包括所述当前目标车辆在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角，估计向量包括所述当前目标车辆在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角；所述车体坐标系为以所述当前目标车辆自身为坐标原点建立的三维坐标系，所述导航坐标系为以所述当前目标车辆初始时刻所在位置为坐标原点建立的三维坐标系。

可选的，所述根据预设滤波器更新方法，基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵，包括：

基于第一系数矩阵和所述目标滤波器上一次输出的上一估计向量确定出当前先验估计向量；

基于所述第一系数矩阵、预设误差协方差矩阵和所述目标滤波器上一次输出的上一协方差矩阵确定出当前先验协方差矩阵；

基于所述当前先验协方差矩阵、第二系数矩阵和预设观测噪声协方差矩阵确定出当前滤波增益；

基于当前观测向量、所述当前先验估计向量、所述当前滤波增益和所述第二系数矩阵确定出当前估计向量；

基于所述当前先验协方差矩阵、所述当前滤波增益和所述第二系数矩阵确定出当前协方差矩阵。

可选的，所述车辆航迹估计方法，还包括：

若为初次更新操作，则所述上一估计向量为初始化后的初始估计向量，所述上一协方差矩阵为初始化后的初始协方差矩阵；其中，所述初始估计向量为零向量，所述初始协方差矩阵为单位矩阵。

可选的，所述车辆航迹估计方法，还包括：

基于当前时间增量构建所述第一系数矩阵；

基于所述导航坐标系变换到所述车体坐标系的第一坐标变换矩阵构建所述第二系数矩阵；其中，基于所述当前目标车辆在所述导航坐标系下的姿态角信息确定出所述第一坐标变换矩阵。

可选的，所述确定当前目标车辆对应的当前观测向量，包括：

获取当前目标车辆在惯性传感器坐标系下的惯性测量值，并利用相应的零偏修正参数和预设比例因子对所述惯性测量值进行修正，得到修正后的所述惯性测量值；

基于所述当前目标车辆的轮速、车轮转动角度、车轮转弯半径、车辆轴距、车辆前轴长度和车轮后轴长度确定出所述当前目标车辆后轴中心点位置的目标线速度和目标角速度；

利用修正后的所述惯性测量值、所述目标线速度、所述目标角速度和所述惯性传感器坐标系变换到所述车体坐标系的第二坐标变换矩阵确定出当前速度观测量、当前加速度观测量和当前角速度观测量；

基于所述传感器坐标系坐标原点的当前重力加速度分量确定出所述当前目标车辆在导航坐标系下的当前姿态角观测量；

基于所述当前速度观测量、所述当前加速度观测量、所述当前角速度观测量和所述当前姿态角观测量构成所述当前目标车辆对应的当前观测向量。

可选的，所述惯性测量值包括陀螺仪输出的角速度惯性测量值和加速度计输出的加速度惯性测量值；

相应的，所述利用相应的零偏修正参数和预设比例因子对所述惯性测量值进行修正，得到修正后的所述惯性测量值，包括：

利用陀螺仪零偏修正参数和相应的陀螺仪比例因子对所述角速度惯性测量值进行修正，得到修正后的所述角速度惯性测量值；

利用加速度计零偏修正参数和相应的加速度计比例因子对所述加速度惯性测量值进行修正，得到修正后的所述加速度惯性测量值。

可选的，所述利用修正后的所述惯性测量值、所述目标线速度、所述目标角速度和所述惯性传感器坐标系变换到所述车体坐标系的第二坐标变换矩阵确定出当前速度观测量、当前加速度观测量和当前角速度观测量，包括：

利用所述目标线速度和所述当前目标车辆后轴中心点位置的转动角度确定出当前速度观测量；

利用修正后的所述加速度惯性测量值以及所述传感器坐标系坐标原点的当前重力加速度分量、当前旋转向心加速度分量、当前线加速度分量和所述惯性传感器坐标系变换到所述车体坐标系的第二坐标变换矩阵确定出当前加速度观测量；

利用所述目标角速度、修正后的所述角速度惯性测量值和所述第二坐标变换矩阵确定出当前角速度观测量；其中，基于惯性传感器的安装姿态角信息确定出所述第二坐标变换矩阵。

可选的，所述车辆航迹估计方法，还包括：

基于所述当前速度观测量、上一速度观测量、修正后的所述惯性测量值和所述车体坐标系变换到所述惯性传感器坐标系的第三坐标变换矩阵、所述传感器坐标系坐标原点的旋转向心加速度分量、所述当前目标车辆后轴中心点位置的线加速度分量确定出所述当前重力加速度分量。

第二方面，本申请公开了一种车辆航迹估计装置，包括：

观测向量确定模块，用于确定当前目标车辆对应的当前观测向量；

滤波器更新模块，用于将所述当前观测向量输入至目标滤波器，以便基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵；其中，所述目标滤波器中设置有预设滤波器更新方法；

航迹信息获取模块，用于基于所述当前估计向量得到所述当前目标车辆的航迹信息；

其中，观测向量包括所述当前目标车辆在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角，估计向量包括所述当前目标车辆在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角；所述车体坐标系为以所述当前目标车辆自身为坐标原点建立的三维坐标系，所述导航坐标系为以所述当前目标车辆初始时刻所在位置为坐标原点建立的三维坐标系。

第三方面，本申请公开了一种电子设备，包括：

存储器，用于保存计算机程序；

处理器，用于执行所述计算机程序，以实现前述公开的车辆航迹估计方法的步骤。

第四方面，本申请公开了一种计算机可读存储介质，用于存储计算机程序；其中，所述计算机程序被处理器执行时实现前述公开的车辆航迹估计方法的步骤。

可见，本申请通过确定当前目标车辆对应的当前观测向量；将所述当前观测向量输入至目标滤波器，以便基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵；其中，所述目标滤波器中设置有预设滤波器更新方法；基于所述当前估计向量得到所述当前目标车辆的航迹信息；其中，观测向量包括所述当前目标车辆在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角，估计向量包括所述当前目标车辆在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角；所述车体坐标系为以所述当前目标车辆自身为坐标原点建立的三维坐标系，所述导航坐标系为以所述当前目标车辆初始时刻所在位置为坐标原点建立的三维坐标系。由此可见，本申请首先确定出当前目标车辆对应的当前观测向量，即当前目标车辆当前在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角等信息，其中，车体坐标系和导航坐标系均为三维坐标系；然后将当前观测向量输入至目标滤波器，以便目标滤波器根据设置的预设滤波器更新方法，基于当前观测向量以及目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到目标滤波器当前输出的当前估计向量和当前协方差矩阵，当前估计向量即为当前目标车辆当前在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角等信息，最后根据当前估计向量即可得到当前目标车辆的三维航迹信息。如此一来，通过上述方案实现了稳定、精准的车辆三维航迹估计。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本申请公开的一种车辆航迹估计方法流程图；

图2为本申请公开的一种坐标系的构建示意图；

图3为本申请公开的一种确定出当前观测向量的流程图；

图4为本申请公开的一种车辆结构示意图；

图5为本申请公开的一种具体的车辆航迹估计方法流程图；

图6为本申请公开的一种航迹、速度和角速度的估计输出流程图；

图7为本申请公开的一种车辆航迹估计装置结构示意图；

图8为本申请公开的一种电子设备结构图。

具体实施方式

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

当前的车辆航迹估计方法依赖较高性能的惯性传感器，较难适用于立体航迹的高精度推算或估计，且稳定性和精度都仍待提高。为此，本申请实施例公开了一种车辆航迹估计方法、装置、设备及介质，能够实现稳定、精准的车辆三维航迹估计。

参见图1所示，本申请实施例公开了一种车辆航迹估计方法，该方法包括：

步骤S11：确定当前目标车辆对应的当前观测向量。

本实施例中，首先确定出当前目标车辆对应的当前观测向量，即当前目标车辆在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角等信息。需要指出的是，车体坐标系(以下简称b系)是以当前目标车辆自身为坐标原点建立的三维坐标系，导航坐标系(以下简称n系)是以当前目标车辆初始时刻所在位置为坐标原点建立的三维坐标系。

具体参见图2所示，车体坐标系见图2中M位置，具体是以车辆后轴中心为原点，过原点垂直底盘平面的法线为b系Z轴，且Z轴正方向指向车顶；取车底盘平面中，过原点和前轴中心点的直线为b系X轴，且X轴正方向从原点的指向车头方向；b系为右手系，如此即可确定b系Y轴；导航坐标系见图2中开始时刻车辆所在的位置，具体是以车辆初始时刻位置的后轴中心点为坐标原点，以过原点垂直于水平面的法线为n系Z轴，且Z轴负方向指向地心；构建平面Γ，其过后轴中心点、前轴中心点和Z轴，取Γ平面中、过原点且垂直与Z轴的直线为n系的X轴，且X轴正方向指向车头方向；n系为右手系，如此即可确定n系Y轴。

需要指出的是，观测向量Z是一个11行1列的单列矩阵，包含车体坐标系(即b坐标系)下的速度

Z＝[v

步骤S12：将所述当前观测向量输入至目标滤波器，以便基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵；其中，所述目标滤波器中设置有预设滤波器更新方法。

本实施例中，将当前观测向量输入至目标滤波器，以便基于当前观测向量以及目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到目标滤波器当前输出的当前估计向量和当前协方差矩阵，当前估计向量即为当前目标车辆当前在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角等信息。其中，目标滤波器中设置有预设滤波器更新方法，以实现如何基于当前观测向量、上一估计向量和上一协方差矩阵更新得到当前估计向量和当前协方差矩阵。

需要指出的是，估计向量X是一个15行1列的单列矩阵，包含导航坐标系(以下简称n系)下的位置(x

X＝[x

步骤S13：基于所述当前估计向量得到所述当前目标车辆的航迹信息。

本实施例中，根据当前估计向量即可得到当前目标车辆的三维航迹信息，即车辆行驶轨迹中各个估计点对应的位置和姿态，具体包括车辆位置坐标，航向角、俯仰角、横滚角等，以便后续进一步强有力的支持为跨层路径规划、控制、感知和定位等。可以理解的是，在跨层工程中，时刻知道车体所在的位置和姿态，是实现自动驾驶跨层的必须条件，本申请实现了竖直方向上的层高定位，可应用于多层停车场中，实现底下停车场和地面的定位，从而实现车辆顺畅过渡，受遮挡的隧道进入和驶出等。

可见，本申请通过确定当前目标车辆对应的当前观测向量；将所述当前观测向量输入至目标滤波器，以便基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵；其中，所述目标滤波器中设置有预设滤波器更新方法；基于所述当前估计向量得到所述当前目标车辆的航迹信息；其中，观测向量包括所述当前目标车辆在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角，估计向量包括所述当前目标车辆在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角；所述车体坐标系为以所述当前目标车辆自身为坐标原点建立的三维坐标系，所述导航坐标系为以所述当前目标车辆初始时刻所在位置为坐标原点建立的三维坐标系。由此可见，本申请首先确定出当前与当前目标车辆对应的当前观测向量，即当前目标车辆当前在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角等信息，其中，车体坐标系和导航坐标系均为三维坐标系；然后将当前观测向量输入至目标滤波器，以便目标滤波器根据设置的预设滤波器更新方法，基于当前观测向量以及目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到目标滤波器当前输出的当前估计向量和当前协方差矩阵，当前估计向量即为当前目标车辆当前在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角等信息，最后根据当前估计向量即可得到当前目标车辆的三维航迹信息。如此一来，通过上述方案实现了稳定、精准的车辆三维航迹估计。

在一个具体实施例中，本申请提供了确定出当前观测向量的具体步骤，参见图3所示，步骤S11可以具体包括：

步骤S111：获取当前目标车辆在惯性传感器坐标系下的惯性测量值，并利用相应的零偏修正参数和预设比例因子对所述惯性测量值进行修正，得到修正后的所述惯性测量值。

本实施例中，首先获取当前目标车辆在惯性传感器坐标系下的惯性测量值，再利用相应的零偏修正参数和预设比例因子对惯性测量值进行修正。需要指出的是，惯性传感器坐标系(以下简称m系)与惯性测量单元的坐标系重合。

在具体实施方式中，上述惯性测量值包括陀螺仪输出的角速度惯性测量值和加速度计输出的加速度惯性测量值；相应的，所述利用相应的零偏修正参数和预设比例因子对所述惯性测量值进行修正，得到修正后的所述惯性测量值，包括：利用陀螺仪零偏修正参数和相应的陀螺仪比例因子对所述角速度惯性测量值进行修正，得到修正后的所述角速度惯性测量值；利用加速度计零偏修正参数和相应的加速度计比例因子对所述加速度惯性测量值进行修正，得到修正后的所述加速度惯性测量值。也即，本实施例根据标定的陀螺仪零偏修正参数、加速度计零偏修正参数(即ω

其中，w

步骤S112：基于所述当前目标车辆的轮速、车轮转动角度、车轮转弯半径、车辆轴距、车辆前轴长度和车轮后轴长度确定出所述当前目标车辆后轴中心点位置的目标线速度和目标角速度。

本实施例中，需要进一步确定出当前目标车辆后轴中心点位置的目标线速度v

式中，v

步骤S113：利用修正后的所述惯性测量值、所述目标线速度、所述目标角速度和所述惯性传感器坐标系变换到所述车体坐标系的第二坐标变换矩阵确定出当前速度观测量、当前加速度观测量和当前角速度观测量。

本实施例中，在具体实施方式中，上述利用修正后的所述惯性测量值、所述目标线速度、所述目标角速度和所述惯性传感器坐标系变换到所述车体坐标系的第二坐标变换矩阵确定出当前速度观测量、当前加速度观测量和当前角速度观测量，包括：利用所述目标线速度和所述当前目标车辆后轴中心点位置的转动角度确定出当前速度观测量；利用修正后的所述加速度惯性测量值以及所述传感器坐标系坐标原点的当前重力加速度分量、当前旋转向心加速度分量、当前线加速度分量和所述惯性传感器坐标系变换到所述车体坐标系的第二坐标变换矩阵确定出当前加速度观测量；利用所述目标角速度、修正后的所述角速度惯性测量值和所述第二坐标变换矩阵确定出当前角速度观测量；其中，基于惯性传感器的安装姿态角信息确定出所述第二坐标变换矩阵。

也即，利用目标线速度v

其中，

如图4所示，P

此外，基于当前速度观测量

其中，基于惯性传感器的安装姿态角信息r

步骤S114：基于所述传感器坐标系坐标原点的当前重力加速度分量确定出所述当前目标车辆在导航坐标系下的当前姿态角观测量。

本实施例中，基于传感器坐标系坐标原点的当前重力加速度分量

步骤S115：基于所述当前速度观测量、所述当前加速度观测量、所述当前角速度观测量和所述当前姿态角观测量构成所述当前目标车辆对应的当前观测向量。

本实施例中，当前目标车辆对应的当前观测向量由当前速度观测量、当前加速度观测量、当前角速度观测量和当前姿态角观测量构成。

可见，本申请实施例提供了确定出当前观测向量的具体方法，在这个过程中采用了紧耦合公式进行计算，进一步提高了计算的稳定性和精度。

参见图5和图6所示，本申请实施例公开了一种具体的车辆航迹估计方法，相对于上一实施例，本实施例对技术方案作了进一步的说明和优化。具体包括：

步骤S21：确定当前目标车辆对应的当前观测向量。

步骤S22：将所述当前观测向量输入至目标滤波器，以便基于第一系数矩阵和所述目标滤波器上一次输出的上一估计向量确定出当前先验估计向量，并基于所述第一系数矩阵、预设误差协方差矩阵和所述目标滤波器上一次输出的上一协方差矩阵确定出当前先验协方差矩阵。

本实施例中，在目标滤波器中，先基于第一系数矩阵F和目标滤波器上一次输出的上一估计向量X

式中，F

其中，基于当前时间增量构建所述第一系数矩阵F，可以理解的是，F是一个15行15列的方阵，其中0

此外，预设误差协方差矩阵Q为15行15列的方阵，Q的表达式为：

其中，

需要指出的是，在预设滤波器更新方法中，若为初次更新操作，则上一估计向量为初始化后的初始估计向量，上一协方差矩阵为初始化后的初始协方差矩阵；其中，初始估计向量为零向量，初始协方差矩阵为单位矩阵。也即，在初始化的时候，需要初始化估计向量X

步骤S23：基于所述当前先验协方差矩阵、第二系数矩阵和预设观测噪声协方差矩阵确定出当前滤波增益。

本实施例中，基于当前先验协方差矩阵

式中，A

其中，基于导航坐标系变换到车体坐标系的第一坐标变换矩阵C

可以理解的是，A是一个11行15列的矩形阵，其中0

此外，预设观测噪声协方差矩阵R为11行11列的方阵，R的表达式为：

式中，

此外，在初始化的时候，还需将标定的方差值

对于观测噪声，若厂家提供了参数，则可直接使用。若没有提供则和噪声标定方法相似，具体如下：首先，记录一定量车辆静止的待标定观测向量的数值样本，记录较高精度设备测量的车辆运动过程中待标定估计向量的数值样本，按照模型公式推导对应的估计向量离散值，同时间序列对比测量值和估计值进而得到过程误差数据样本。

其次，按照正态分布

步骤S24：基于当前观测向量、所述当前先验估计向量、所述当前滤波增益和所述第二系数矩阵确定出当前估计向量，并基于所述当前先验协方差矩阵、所述当前滤波增益和所述第二系数矩阵确定出当前协方差矩阵。

本实施例中，基于当前观测向量Z

基于当前先验协方差矩阵

步骤S25：基于所述当前估计向量得到所述当前目标车辆的航迹信息。

本实施例中，基于当前估计向量得到当前目标车辆的车辆三维航迹估计值、速度估计值、角速度估计值。

其中，关于上述步骤S21和S25更加具体的处理过程可以参考前述实施例中公开的相应内容，在此不再进行赘述。

可见，本申请实施例在目标滤波器中，根据第一系数矩阵、第二系数矩阵、预设误差协方差矩阵、预设观测噪声协方差矩阵完成目标滤波器的更新操作，以基于当前观测向量以及目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到目标滤波器当前输出的当前估计向量和当前协方差矩阵，从而基于当前估计向量得到当前目标车辆的车辆三维航迹估计值、速度估计值、角速度估计值，实现了稳定、精准的车辆三维航迹估计。

参见图7所示，本申请实施例公开了一种车辆航迹估计装置，该装置包括：

观测向量确定模块11，用于确定当前目标车辆对应的当前观测向量；

滤波器更新模块12，用于将所述当前观测向量输入至目标滤波器，以便基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵；其中，所述目标滤波器中设置有预设滤波器更新方法；

航迹信息获取模块13，用于基于所述当前估计向量得到所述当前目标车辆的航迹信息；

其中，观测向量包括所述当前目标车辆在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角，估计向量包括所述当前目标车辆在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角；所述车体坐标系为以所述当前目标车辆自身为坐标原点建立的三维坐标系，所述导航坐标系为以所述当前目标车辆初始时刻所在位置为坐标原点建立的三维坐标系。

可见，本申请通过确定当前与当前目标车辆对应的当前观测向量；将所述当前观测向量输入至目标滤波器，以便基于所述当前观测向量以及所述目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到所述目标滤波器当前输出的当前估计向量和当前协方差矩阵；其中，所述目标滤波器中设置有预设滤波器更新方法；基于所述当前估计向量得到所述当前目标车辆的航迹信息；其中，观测向量包括所述当前目标车辆在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角，估计向量包括所述当前目标车辆在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角；所述车体坐标系为以所述当前目标车辆自身为坐标原点建立的三维坐标系，所述导航坐标系为以所述当前目标车辆初始时刻所在位置为坐标原点建立的三维坐标系。由此可见，本申请首先确定出当前目标车辆对应的当前观测向量，即当前目标车辆当前在车体坐标系下的速度、加速度、角速度和在导航坐标系下的姿态角等信息，其中，车体坐标系和导航坐标系均为三维坐标系；然后将当前观测向量输入至目标滤波器，以在目标滤波器中根据设置的预设滤波器更新方法，基于当前观测向量以及目标滤波器上一次输出的上一估计向量和上一协方差矩阵得到目标滤波器当前输出的当前估计向量和当前协方差矩阵，当前估计向量即为当前目标车辆当前在导航坐标系下的位置坐标、速度、加速度、角速度和姿态角等信息，最后根据当前估计向量即可得到当前目标车辆的三维航迹信息。如此一来，通过上述方案实现了稳定、精准的车辆三维航迹估计。

图8为本申请实施例提供的一种电子设备的结构示意图。具体可以包括：至少一个处理器21、至少一个存储器22、电源23、通信接口24、输入输出接口25和通信总线26。其中，所述存储器22用于存储计算机程序，所述计算机程序由所述处理器21加载并执行，以实现前述任一实施例公开的由电子设备执行的车辆航迹估计方法中的相关步骤。

本实施例中，电源23用于为电子设备20上的各硬件设备提供工作电压；通信接口24能够为电子设备20创建与外界设备之间的数据传输通道，其所遵循的通信协议是能够适用于本申请技术方案的任意通信协议，在此不对其进行具体限定；输入输出接口25，用于获取外界输入数据或向外界输出数据，其具体的接口类型可以根据具体应用需要进行选取，在此不进行具体限定。

其中，处理器21可以包括一个或多个处理核心，比如4核心处理器、8核心处理器等。处理器21可以采用DSP(Digital Signal Processing，数字信号处理)、FPGA(Field－Programmable Gate Array，现场可编程门阵列)、PLA(Programmable Logic Array，可编程逻辑阵列)中的至少一种硬件形式来实现。处理器21也可以包括主处理器和协处理器，主处理器是用于对在唤醒状态下的数据进行处理的处理器，也称CPU(Central ProcessingUnit，中央处理器)；协处理器是用于对在待机状态下的数据进行处理的低功耗处理器。在一些实施例中，处理器21可以在集成有GPU(Graphics Processing Unit，图像处理器)，GPU用于负责显示屏所需要显示的内容的渲染和绘制。一些实施例中，处理器21还可以包括AI(Artificial Intelligence，人工智能)处理器，该AI处理器用于处理有关机器学习的计算操作。

另外，存储器22作为资源存储的载体，可以是只读存储器、随机存储器、磁盘或者光盘等，其上所存储的资源包括操作系统221、计算机程序222及数据223等，存储方式可以是短暂存储或者永久存储。

其中，操作系统221用于管理与控制电子设备20上的各硬件设备以及计算机程序222，以实现处理器21对存储器22中海量数据223的运算与处理，其可以是Windows、Unix、Linux等。计算机程序222除了包括能够用于完成前述任一实施例公开的由电子设备20执行的车辆航迹估计方法的计算机程序之外，还可以进一步包括能够用于完成其他特定工作的计算机程序。数据223除了可以包括电子设备接收到的由外部设备传输进来的数据，也可以包括由自身输入输出接口25采集到的数据等。

进一步的，本申请实施例还公开了一种计算机可读存储介质，所述存储介质中存储有计算机程序，所述计算机程序被处理器加载并执行时，实现前述任一实施例公开的车辆航迹估计方法步骤。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其它实施例的不同之处，各个实施例之间相同或相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

本领域技术人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。本领域技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上对本发明所提供的一种车辆航迹估计方法、装置、设备及存储介质进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。