一种多传感器状态估计方法、装置及终端设备与流程

本申请属于数据处理技术领域，尤其涉及一种多传感器状态估计方法、装置及终端设备。

背景技术：

状态估计是机器人和自动驾驶定位导航领域的关键技术，状态估计是指通过一个或多个传感器的测量信息，结合系统的数学模型，估计系统的状态变量的当前值或历史值的过程，系统的主要状态变量可以为机器人或车辆当前的位置、姿态、线速度、角速度等信息。

在进行状态估计时，使用多传感器对系统进行冗余测量能够得到比单一传感器测量更准确的状态估计，在使用多传感器进行冗余测量时，当前的主流方案是基于卡尔曼滤波器进行多传感器的信息融合。

但是，现有使用卡尔曼滤波进行多传感器融合时，需要进行时间同步，会丢失部分测量数据、时间同步难度大且卡尔曼滤波器的输出频率低，难以满足对动态性能和实时性能要求高的系统。

技术实现要素：

有鉴于此，本申请实施例提供了一种多传感器状态估计方法、装置及终端设备，以解决现有使用卡尔曼滤波进行多传感器融合时，需要进行时间同步，会丢失部分测量数据、时间同步难度大且卡尔曼滤波器的输出频率低，难以满足对动态性能和实时性能要求高的系统的问题。

本申请实施例的第一方面提供了一种多传感器状态估计方法，包括：

在每次状态估计循环中，以各个传感器消息队列中未使用的传感器消息的时间戳中最早的时间戳作为初始时间戳，取出各个传感器消息队列中时间戳晚于或等于所述初始时间戳的传感器消息并根据时间戳的先后顺序排列成数据更新队列，其中，所述传感器消息队列用于接收参与状态估计的传感器采集到的传感器消息；

删除状态估计队列中时间戳晚于所述初始时间戳的系统状态估计值，其中，所述状态估计队列用于存储根据传感器消息预测或更新得到的系统状态估计值；

依次取出所述数据更新队列中的传感器消息，所述传感器消息包括预测类数据和更新类数据，以取出的传感器消息的时间戳作为第一时间戳；

当取出的传感器消息为预测类数据时，根据卡尔曼滤波的预测算法，使用所述状态估计队列中最后一个系统状态估计值和所述取出的传感器消息预测所述第一时间戳对应的系统状态估计值；

当取出的传感器消息为更新类数据时，获取所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法，使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值；

当所述数据更新队列中的传感器消息均用于预测或更新所述状态估计队列中的系统状态估计值后，进行下一次状态估计循环；

以预设频率检测所述状态估计队列中时间戳最晚的系统状态估计值并输出。

本申请实施例的第二方面提供了一种多传感器状态估计装置，包括：

更新队列模块，用于在每次状态估计循环中，以各个传感器消息队列中未使用的传感器消息的时间戳中最早的时间戳作为初始时间戳，取出各个传感器消息队列中时间戳晚于或等于所述初始时间戳的传感器消息并根据时间戳的先后顺序排列成数据更新队列，其中，所述传感器消息队列用于接收参与状态估计的传感器采集到的传感器消息；

状态删除模块，用于删除状态估计队列中时间戳晚于所述初始时间戳的系统状态估计值，其中，所述状态估计队列用于存储根据传感器消息预测或更新得到的系统状态估计值；

数据取用模块，用于依次取出所述数据更新队列中的传感器消息，所述传感器消息包括预测类数据和更新类数据，以取出的传感器消息的时间戳作为第一时间戳；

状态预测模块，用于当取出的传感器消息为预测类数据时，根据卡尔曼滤波的预测算法，使用所述状态估计队列中最后一个系统状态估计值和所述取出的传感器消息预测所述第一时间戳对应的系统状态估计值；

状态更新模块，用于当取出的传感器消息为更新类数据时，获取所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法，使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值；

状态循环模块，用于当所述数据更新队列中的传感器消息均用于预测或更新所述状态估计队列中的系统状态估计值后，进行下一次状态估计循环；

状态输出模块，用于以预设频率检测所述状态估计队列中时间戳最晚的系统状态估计值并输出。

本申请实施例的第三方面提供了一种终端设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述方法的步骤。

本申请实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述方法的步骤。

本申请实施例与现有技术相比存在的有益效果是：

本申请的多传感器状态估计方法中，在进行卡尔曼滤波时，不必进行时间同步，接收到的所有传感器消息均用于状态估计，预测或更新对应的系统状态估计值，可以避免丢失传感器测量得到的数据，得到更准确的系统状态估计值，并且由于无需进行时间同步，所以卡尔曼滤波循环中，循环的周期不必与测量频率最低的传感器同步，状态估计的输出频率可以自由选择，甚至达到与测量频率最高的传感器相同的输出频率，极大地提高了算法的实时性，可以满足对动态性能和实时性能要求高的系统，解决了现有使用卡尔曼滤波进行多传感器融合时，需要进行时间同步，会丢失部分测量数据、时间同步难度大且卡尔曼滤波器的输出频率低，难以满足对动态性能和实时性能要求高的系统的问题。

附图说明

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

图1是本申请实施例提供的多传感器状态估计方法的实现流程示意图；

图2是本申请实施例提供的多传感器状态估计装置的示意图；

图3是本申请实施例提供的终端设备的示意图；

图4是本申请实施例提供的状态估计队列的更新示意图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。

为了说明本申请所述的技术方案，下面通过具体实施例来进行说明。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。如在本申请说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

如在本说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。

下面对本申请实施例提供的一种多传感器状态估计方法进行描述，请参阅附图1，本申请实施例中的多传感器状态估计方法包括：

步骤s101、在每次状态估计循环中，以各个传感器消息队列中未使用的传感器消息的时间戳中最早的时间戳作为初始时间戳，取出各个传感器消息队列中时间戳晚于或等于所述初始时间戳的传感器消息并根据时间戳的先后顺序排列成数据更新队列，其中，所述传感器消息队列用于接收参与状态估计的传感器采集到的传感器消息；

在每次状态估计循环中，检测当前各个传感器消息队列中接收到的未使用的传感器消息中时间戳最早的传感器消息，以该传感器消息的时间戳作为初始时间戳。

然后可以取出各个传感器消息队列中所有时间戳晚于或等于初始时间戳的传感器消息，根据时间戳的先后顺序对传感器消息进行排序，得到数据更新队列，例如，上一次循环中数据更新队列中的传感器消息为{a1，b1，a2，a3}，本次状态估计循环中，检测到新接收到的传感数据c1的时间戳早于a1，则本次循环的数据更新队列可能为{c1、a1，b1，a2，a3，b2}，c1和b2为新接收到的传感器消息，a1、b1、a2、a3虽然在上一次循环中已经被使用，但是a1、b1、a2、a3的时间戳均晚于c1的时间戳，因此本次循环中也会被取出再次使用。

传感器消息队列用于接收参与状态估计的传感器采集到的传感器消息。

步骤s102、删除状态估计队列中时间戳晚于所述初始时间戳的系统状态估计值，其中，所述状态估计队列用于存储根据传感器消息预测或更新得到的系统状态估计值；

状态估计队列用于存储根据传感器消息预测或更新得到的系统状态估计值，每一个系统状态估计值的时间戳与用于预测或更新该系统状态估计值的传感器消息的时间戳相同，在每一次状态估计循环中，获取到初始时间戳后，删除状态估计队列中时间戳晚于所述初始时间戳的系统状态估计值，例如，状态估计队列可能表示为{k1，k2，k3，k4，k5，k6}，其中，k3至k6的时间戳均晚于初始时间戳，此时可以将其删除，然后根据本次状态估计循环的数据更新队列进行系统状态估计值的预测或更新。

步骤s103、依次取出所述数据更新队列中的传感器消息，所述传感器消息包括预测类数据和更新类数据，以取出的传感器消息的时间戳作为第一时间戳；

删除了状态估计队列中失效的系统状态估计值后，可以依次取出数据更新队列中的传感器消息，根据取出的传感器消息进行系统状态估计值的预测或更新，传感器消息包括预测类数据和更新类数据，例如，可以选取一个传感器作为预测类传感器，则该传感器测量到的传感器消息为预测类数据，其他传感器作为更新类传感器，则其他传感测量到的传感器消息为更新类数据。取出传感器消息时，以当前取出的传感器消息的时间戳作为第一时间戳。

参与状态估计的传感器包括更新类传感器和预测类传感器，预测类传感器通常只设置一个，更新类传感器可以设置一个或多个，预测类传感器采集到的传感器消息为预测类数据，更新类传感器采集到的传感器消息为更新类数据。

步骤s104、当取出的传感器消息为预测类数据时，根据卡尔曼滤波的预测算法，使用所述状态估计队列中最后一个系统状态估计值和所述取出的传感器消息预测所述第一时间戳对应的系统状态估计值；

当取出的传感器消息为预测类数据时，可以根据卡尔曼滤波的预测算法，根据状态估计队列中最后一个系统状态估计值和取出的传感器消息预测第一时间戳对应的系统状态估计值，预测得到的系统状态估计值成为状态估计队列中新的最后一个系统状态估计值。

其中，卡尔曼滤波的算法中包括预测算法和更新算法，通过预测算法和更新算法可以使用系统的状态方程和测量方程进行状态估计。

步骤s105、当取出的传感器消息为更新类数据时，获取所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法，使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值；

当取出的传感器消息为更新类数据时，获取第一时间戳对应的系统状态估计值，然后根据卡尔曼滤波的更新算法，使用取出的传感器消息更新第一时间戳对应的系统状态估计值，得到新的系统状态估计值，更新得到的新的系统状态估计值成为状态估计队列中新的最后一个系统状态估计值。

步骤s106、当所述数据更新队列中的传感器消息均用于预测或更新所述状态估计队列中的系统状态估计值后，进行下一次状态估计循环；

依次取出数据更新队列中的传感器消息预测或更新状态估计队列中的系统状态估计值，当数据更新队列中所有传感器消息均被使用后，进入下一次状态估计循环。

步骤s107、以预设频率检测所述状态估计队列中时间戳最晚的系统状态估计值并输出。

当需要输出系统状态估计值时，可以以预设频率对状态估计队列进行扫描，检测当前状态估计队列中时间戳最晚的系统状态估计值，即状态估计队列中的的最后一个系统状态估计值，并且在检测后输出该系统状态估计值。

预设频率根据实际需求进行设置，例如，若对实时性的要求高，则可以将预设频率设置为测量频率最高的传感器的测量频率一致，若对实时性的要求不高，则可以设置为其他较低的频率，具体设置方案可以根据实际需求进行选择。

输出系统状态估计值的过程和状态估计循环的过程可以为同步的过程，也可以为异步的过程，两者可以采用相同的频率，也可以采用不同的频率，例如，状态估计循环可以按照测量频率最高的传感器的频率作为循环频率，而预设频率可以与循环频率一致，也可以与循环频率不一致，可以选择每一次状态估计循环后输出时间戳最晚的系统状态估计值，也可以经历了多次状态估计循环后才输出一次时间戳最晚的系统状态估计值。

进一步地，所述当取出的传感器消息为更新类数据时，获取所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法，使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值具体包括：

a1、当取出的传感器消息为更新类数据时，判断所述第一时间戳是否存在对应的系统状态估计值；

当取出的传感器消息为更新类数据时，可以先判断第一时间戳是否存在对应的系统状态估计值，在实际的应用中，当系统状态估计值的时间戳与第一时间戳的差值小于预设时间阈值时，可以认为该系统状态估计值为第一时间戳对应的系统状态估计值。预设时间阈值可以根据实际应用过程中精度需求以及工程经验进行设置。

a2、当所述第一时间戳存在对应的系统状态估计值时，根据卡尔曼滤波的更新算法使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值。

如果第一时间戳存在对应的系统状态估计值，可以直接根据卡尔曼滤波的更新算法，使用取出的传感器消息更新第一时间戳对应的系统状态估计值，得到第一时间戳对应的新的系统状态估计值。

进一步地，所述当取出的传感器消息为更新类数据时，获取所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法，使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值还包括：

a3、当所述第一时间戳不存在对应的系统状态估计值时，根据所述状态估计队列中最后一个系统状态估计值进行插值计算，得到所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法使用所述传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值。

当第一时间戳不存在对应的系统状态估计值时，可以根据状态估计队列中最后一个系统状态估计值进行插值计算，根据系统的运动学方程，通过线性插值等方式，得到第一时间戳对应的系统状态估计值，然后根据卡尔曼滤波的更新算法，使用传感器消息更新第一时间戳对应的系统状态估计值，得到新的系统状态估计值，更新得到的新的系统状态估计值成为状态估计队列中新的最后一个系统状态估计值。

以图4为例，数据更新队列中存在3个预测类数据和2个更新类数据，第一个传感器消息为预测类数据时，根据系统状态估计值k以及第一个预测类数据预测系统状态估计值k+1；第二个数据依然为预测类数据，根据系统状态估计值k+1和第二个预测类数据预测系统状态估计值k+2；第三个数据为更新类数据，则根据系统状态估计值k+2进行插值计算，得到系统状态估计值kk1，然后使用更新类数据1对kk1进行更新，得到系统状态估计值k+3；第四个数据为更新类数据，则根据系统状态估计值k+3进行插值计算，得到系统状态估计值kk2，使用更新类数据2对kk2进行更新，得到系统状态估计值k+4；第五个数据为预测类数据，则根据系统状态估计值k+4和第三个预测类数据预测系统状态估计值k+5。

进一步地，所述方法还包括：

b1、删除保留时长大于预设时长的传感器消息。

传感器消息的保留时长为传感器消息的时间戳与当前时间戳的时间间隔，表示该传感器消息已经保留的时间，当系统中存储的传感器消息的保留时长大于预设时长时，可以认为该传感器消息为过期数据，为了节约系统的存储空间，可以删除过期数据。预设时长可以根据实际需求进行设置，例如，可以将预设时长设置为5分钟、10分钟等。

进一步地，在所述在每次状态估计循环中，以各个传感器消息队列中未使用的传感器消息的时间戳中最早的时间戳作为初始时间戳，取出各个传感器消息队列中时间戳晚于或等于所述初始时间戳的传感器消息并根据时间戳的先后顺序排列成数据更新队列，其中，所述传感器消息队列用于接收参与状态估计的传感器采集到的传感器消息之前还包括：

c1、设置预设数量的传感器消息队列，使用传感器消息队列接收对应传感器的传感器消息，其中，预设数量为所述参与卡尔曼滤波的传感器的数量，一个传感器消息队列对应一个参与卡尔曼滤波的传感器。

接收传感器测量的数据时，可以设置预设数量的传感器消息队列，预设数量为参与卡尔曼滤波的传感器的数量，每一个传感器对应一个传感器消息队列，避免传感器消息队列中不同传感器的时间戳混乱，例如，a传感器的测量数据处理较快，b传感器的测量数据处理较慢，可能a传感器时间戳为30'17”的传感器消息，在30'31”就处理完毕送入传感器消息队列，b传感器时间戳为30'03”的传感器消息，在30'35”才送入传感器消息队列，如果采用单一队列，则不方便检测未使用的传感器消息中时间戳最早的传感器消息，需要依次检测每一个新接收到的传感器消息的时间戳，例如，新接收到的传感器消息为{a1、b1、a2、b2、c1}，则此时需要依次检测各个新接收到的传感器消息的时间戳；当一个传感器消息对应的一个传感器消息队列时，每个传感器消息队列中接收到的传感器消息必然按照时间戳的先后顺序排列，此时只要对每个传感器消息队列中新接收到的传感器消息中最早的传感器消息，即可检测到未使用的传感器消息中时间戳最早的传感器消息，例如，各个传感器消息队列新接收到的传感器消息为{a1，a2}、{b1}、{c1，c2}，则只需要取出a1、b1和c1进行时间戳比较即可得到未使用的传感器消息中时间戳最早的传感器消息。

使用传感器消息队列接收传感器消息时，接收传感器消息的过程与状态估计循环的过程为异步进行的过程，当进行状态估计循环时，传感器消息队列依然会持续接收传感器消息，接收到的新的传感器消息用于下一次状态估计循环中。

本实施例的多传感器状态估计方法中，在进行卡尔曼滤波时，不必进行时间同步，接收到的所有传感器消息均用于状态估计，预测或更新对应的系统状态估计值，可以避免丢失传感器测量得到的数据，得到更准确的系统状态估计值，并且由于无需进行时间同步，所以卡尔曼滤波循环中，循环的周期不必与测量频率最低的传感器同步，状态估计的输出频率可以自由选择，甚至达到与测量频率最高的传感器相同的输出频率，极大地提高了算法的实时性，可以满足对动态性能和实时性能要求高的系统，解决了现有使用卡尔曼滤波进行多传感器融合时，需要进行时间同步，会丢失部分测量数据、时间同步难度大且卡尔曼滤波器的输出频率低，难以满足对动态性能和实时性能要求高的系统的问题。

当使用更新类数据进行系统状态估计值的更新时，可以先检测是否存在与第一时间戳对应的系统状态估计值，若存在，则根据卡尔曼滤波的更新算法使用传感器消息对系统状态估计值进行更新，若不存在，则通过插值计算得到第一时间戳对应的系统状态估计值，然后再进行更新。

当传感器的时间戳过早时，可以将该传感器消息判定为过期数据，删除过期数据节约存储空间。

接收传感器消息时，可以采用多个传感器消息队列，每个传感器消息队列对应的一个传感器，有序接收传感器消息，提高数据处理和调用的效率。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。

本申请实施例提供了一种多传感器状态估计装置，为便于说明，仅示出与本申请相关的部分，如图2所示，多传感器状态估计装置包括，

更新队列模块201，用于在每次状态估计循环中，以各个传感器消息队列中未使用的传感器消息的时间戳中最早的时间戳作为初始时间戳，取出各个传感器消息队列中时间戳晚于或等于所述初始时间戳的传感器消息并根据时间戳的先后顺序排列成数据更新队列，其中，所述传感器消息队列用于接收参与状态估计的传感器采集到的传感器消息；

状态删除模块202，用于删除状态估计队列中时间戳晚于所述初始时间戳的系统状态估计值，其中，所述状态估计队列用于存储根据传感器消息预测或更新得到的系统状态估计值；

数据取用模块203，用于依次取出所述数据更新队列中的传感器消息，所述传感器消息包括预测类数据和更新类数据，以取出的传感器消息的时间戳作为第一时间戳；

状态预测模块204，用于当取出的传感器消息为预测类数据时，根据卡尔曼滤波的预测算法，使用所述状态估计队列中最后一个系统状态估计值和所述取出的传感器消息预测所述第一时间戳对应的系统状态估计值；

状态更新模块205，用于当取出的传感器消息为更新类数据时，获取所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法，使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值；

状态循环模块206，用于当所述数据更新队列中的传感器消息均用于预测或更新所述状态估计队列中的系统状态估计值后，进行下一次状态估计循环；

状态输出模块207，用于以预设频率检测所述状态估计队列中时间戳最晚的系统状态估计值并输出。

进一步地，所述状态更新模块206具体包括：

状态判断子模块，用于当取出的传感器消息为更新类数据时，判断所述第一时间戳是否存在对应的系统状态估计值；

第一更新子模块，用于当所述第一时间戳存在对应的系统状态估计值时，根据卡尔曼滤波的更新算法使用所述传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值。

进一步地，所述状态更新模块206还包括：

第二更新子模块，用于当所述第一时间戳不存在对应的系统状态估计值时，根据所述状态估计队列中最后一个系统状态估计值进行插值计算，得到所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法使用所述传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值。

进一步地，所述装置还包括：

过期删除模块，用于删除保留时长大于预设时长的传感器消息。

进一步地，所述装置还包括：

传感队列模块，用于设置预设数量的传感器消息队列，其中，预设数量为所述参与卡尔曼滤波的传感器的数量，一个传感器消息队列对应一个参与卡尔曼滤波的传感器。

进一步地，所述参与状态估计的传感器包括更新类传感器和预测类传感器。

需要说明的是，上述装置/单元之间的信息交互、执行过程等内容，由于与本申请方法实施例基于同一构思，其具体功能及带来的技术效果，具体可参见方法实施例部分，此处不再赘述。

图3是本申请实施例提供的终端设备的示意图。如图3所示，该实施例的终端设备3包括：处理器30、存储器31以及存储在所述存储器31中并可在所述处理器30上运行的计算机程序32。所述处理器30执行所述计算机程序32时实现上述多传感器状态估计方法实施例中的步骤，例如图1所示的步骤s101至s107。或者，所述处理器30执行所述计算机程序32时实现上述各装置实施例中各模块/单元的功能，例如图2所示模块201至207的功能。

示例性的，所述计算机程序32可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器31中，并由所述处理器30执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序32在所述终端设备3中的执行过程。例如，所述计算机程序32可以被分割成更新队列模块、状态删除模块、数据取用模块、状态预测模块、状态更新模块、状态循环模块以及状态输出模块，各模块具体功能如下：

更新队列模块，用于在每次状态估计循环中，以各个传感器消息队列中未使用的传感器消息的时间戳中最早的时间戳作为初始时间戳，取出各个传感器消息队列中时间戳晚于或等于所述初始时间戳的传感器消息并根据时间戳的先后顺序排列成数据更新队列，其中，所述传感器消息队列用于接收参与状态估计的传感器采集到的传感器消息；

状态删除模块，用于删除状态估计队列中时间戳晚于所述初始时间戳的系统状态估计值，其中，所述状态估计队列用于存储根据传感器消息预测或更新得到的系统状态估计值；

数据取用模块，用于依次取出所述数据更新队列中的传感器消息，所述传感器消息包括预测类数据和更新类数据，以取出的传感器消息的时间戳作为第一时间戳；

状态预测模块，用于当取出的传感器消息为预测类数据时，根据卡尔曼滤波的预测算法，使用所述状态估计队列中最后一个系统状态估计值和所述取出的传感器消息预测所述第一时间戳对应的系统状态估计值；

状态更新模块，用于当取出的传感器消息为更新类数据时，获取所述第一时间戳对应的系统状态估计值，根据卡尔曼滤波的更新算法，使用所述取出的传感器消息更新所述第一时间戳对应的系统状态估计值，得到所述第一时间戳对应的新的系统状态估计值；

状态循环模块，用于当所述数据更新队列中的传感器消息均用于预测或更新所述状态估计队列中的系统状态估计值后，进行下一次状态估计循环；

状态输出模块，用于以预设频率检测所述状态估计队列中时间戳最晚的系统状态估计值并输出。

所述终端设备3可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括，但不仅限于，处理器30、存储器31。本领域技术人员可以理解，图3仅仅是终端设备3的示例，并不构成对终端设备3的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器30可以是中央处理单元(centralprocessingunit，cpu)，还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现成可编程门阵列(field-programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器31可以是所述终端设备3的内部存储单元，例如终端设备3的硬盘或内存。所述存储器31也可以是所述终端设备3的外部存储设备，例如所述终端设备3上配备的插接式硬盘，智能存储卡(smartmediacard,smc)，安全数字(securedigital,sd)卡，闪存卡(flashcard)等。进一步地，所述存储器31还可以既包括所述终端设备3的内部存储单元也包括外部存储设备。所述存储器31用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器31还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

在本申请所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。