CN114124033A

CN114124033A - 卡尔曼滤波器的实现方法、装置、存储介质和设备

Info

Publication number: CN114124033A
Application number: CN202111183665.7A
Authority: CN
Inventors: 焦敬恩; 王东峰; 华斌; 殷宏杰
Original assignee: Jiaxing Jusu Electronic Technology Co ltd; Beijing Transmicrowave Technology Co ltd
Current assignee: Jiaxing Jusu Electronic Technology Co ltd; Beijing Transmicrowave Technology Co ltd
Priority date: 2021-10-11
Filing date: 2021-10-11
Publication date: 2022-03-01

Abstract

本发明实施例提供一种卡尔曼滤波器实现的方法、存储介质和电子装置，所述方法包括：获取并初始化系统参数，获取量测信息，根据量测信息和后验观测噪声统计量计算有效卡尔曼增益，根据线性观测模型和多元高斯分布的线性性质得到量测的概率密度函数，根据概率密度函数计算通过重要性采样获得的采样点的后验观测噪声分布的期望，对后验观测噪声统计量进行更新，根据所述有效卡尔曼增益以及更新后的后验观测噪声统计量对状态协方差矩阵进行更新，根据更新后的状态协方差矩阵对状态进行更新，重复执行获取量测信息至对状态协方差矩阵进行更新的步骤，直至状态实现收敛。本发明能够提高卡尔曼滤波器的计算速度和估计精度。

Description

卡尔曼滤波器的实现方法、装置、存储介质和设备

技术领域

本发明涉及滤波技术领域，尤其涉及一种卡尔曼滤波器的实现方法、装置、存储介质和设备。

背景技术

设计一个最优滤波器需要对包含潜在随机过程的统计模型有准确的了解，然而，在许多工程应用中，由于复杂性、实际局限性、有限的数据等原因，不可能了解模型的全部信息。因此，设计一个鲁棒滤波器是很有意义的，它相对于一类与部分先验知识相容的不确定性模型有良好的性能。卡尔曼滤波器在导航、目标跟踪等工中得到了广泛的应用，该滤波器结构简单，可以在时域内实现，但对噪声统计的精度要求很高。

随着对卡尔曼滤波技术的深入研究，各种类型的卡尔曼滤波算法不断地被提出。经典卡尔曼滤波算法是在模型已知的条件下工作的，但在面对未知模型时，经典卡尔曼滤波算法无法对状态作出精确的估计。然而，目前改进后的卡尔曼滤波算法往往伴随着大量的计算，这就要求计算机具有超高的计算速度，也就无法满足某些情况下的实时性要求。因此，计算速度和估计精度已经成为经典卡尔曼滤波算法进一步优化的目标。

鲁棒卡尔曼滤波算法有很多，大多数涉及自适应卡尔曼滤波，其目的是同时估计噪声统计和状态。自适应卡尔曼滤波器的问题是，它们通常需要大量的观测数据来调整参数并获得可靠的性能。然而，大量的数据会给计算带来负担。

发明内容

本发明提供的卡尔曼滤波器的实现方法、装置、存储介质和设备，能够提高卡尔曼滤波器的计算速度和估计精度。

第一方面，本发明实施例提供一种卡尔曼滤波器的实现方法，所述方法包括：

获取并初始化系统参数，所述系统参数包括先验信息和后验观测噪声统计量；

获取量测信息，根据所述量测信息和后验观测噪声统计量计算有效卡尔曼增益；

根据线性观测模型和多元高斯分布的线性性质得到量测的概率密度函数；

根据所述概率密度函数计算通过重要性采样获得的采样点的后验观测噪声分布的期望；

根据所有历史时刻后验观测噪声分布的期望对所述后验观测噪声统计量进行更新，获得更新后的后验观测噪声统计量；

根据所述有效卡尔曼增益后更新后的后验观测噪声统计量对状态协方差矩阵进行更新；

根据更新后的状态协方差矩阵对状态进行更新，重复执行获取量测信息至状态信息进行更新的步骤，直至状态实现收敛。

进一步的，所述先验信息包括过程噪声、状态转移矩阵、量测转移矩阵。

进一步的，所述初始化系统参数包括为初始状态、初始状态协方差矩阵、后验观测噪声统计量赋初值。

进一步的，所述根据线性观测模型和多元高斯分布的线性性质得到量测的概率密度函数包括：

利用先验信息获得线性观测模型

根据所述线性观测模型和多元高斯分布

的线性性质，得到k 时刻以噪声参数θ为条件的量测信息，即

将

的概率密度函数近似为以不确定噪声参数θ为条件的

的概率密度函数，即

其中，

表示k时刻以噪声参数θ为条件的量测信息，H_k表示k时刻的观测转移矩阵，x_k表示真实状态，

表示k时刻的随机观测噪声，

表示误差协方差矩阵，

表示估计状态。

进一步的，所述根据所述概率密度函数计算通过重要性采样获得的采样点的后验观测噪声分布的期望包括：

对新选取的高斯分布进行重要性采样，获得采样点；

根据所述概率密度函数计算所述采样点的后验观测噪声分布的期望。

第二方面，本发明提供了一种卡尔曼滤波器的实现装置，所述装置包括：

初始化模块，用于获取并初始化系统参数，所述系统参数包括先验信息和后验观测噪声统计量；

卡尔曼增益计算模块，用于获取量测信息，根据所述量测信息和后验观测噪声统计量计算有效卡尔曼增益；

概率密度函数计算模块，用于根据线性观测模型和多元高斯分布的线性性质得到量测的概率密度函数；

后验观测噪声分布期望计算模块，用于根据所述概率密度函数计算通过重要性采样获得的采样点的后验观测噪声分布的期望；

后验观测噪声统计量更新模块，用于根据所有历史时刻后验观测噪声分布的期望对所述后验观测噪声统计量进行更新，获得更新后的后验观测噪声统计量；

状态协方差矩阵更新模块，用于根据所述有效卡尔曼增益后更新后的后验观测噪声统计量对状态协方差矩阵进行更新；

迭代模块，用于根据更新后的状态协方差矩阵对状态进行更新，重复执行获取量测信息至状态信息进行更新的步骤，直至状态实现收敛。

第三方面，本发明提供了一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行第一方面所述的方法。

第四方面，本发明提供了一种设备，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行第一方面所述的方法。

本发明提供的技术方案，利用多元高斯分布线性变换的性质，得到k时刻量测的概率密度函数，此函数可近似为以不确定噪声参数为条件的k时刻量测的概率密度函数；然后利用重要性采样的思想计算出后验噪声分布的期望；最后根据每一时刻的后验期望来计算k时刻的统计平均值，并利用此后验有效噪声统计量去对下一时刻的状态进行估计，最终得到优化后的估计值。

附图说明

图1是本发明实施例提供的一种卡尔曼滤波器的实现方法的流程图；

图2是本发明实施例提供的卡尔曼滤波器实现方法中得到量测的概率密度函数的实现方法流程图；

图3是高斯分布线性变换性质的示意图；

图4是本发明实施例提供的卡尔曼滤波器实现方法中利用重要性采样获得采样点的示意图；

图5是本发明另一实施例提供的卡尔曼滤波器实现装置的结构示意图。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部份实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

为满足卡尔曼滤波器计算速度和估计精度的目标，本发明实施例可以假设状态、量测和噪声服从高斯分布，且噪声的先验分布已知。利用多元高斯分布线性变换的性质，得到K时刻量测的概率密度函数，此函数可近似为以不确定噪声参数为条件的K时刻量测的概率密度函数，然后利用重要性采样的思想计算出后验观测噪声分布的期望，最后根据每一时刻的后验观测噪声分布的期望来计算K时刻的统计平均值，并利用此后验观测噪声分布的期望去对下一时刻的状态进行估计，最终得到优化后的估计值。提高了卡尔曼滤波器的计算速度和估计精度。

参见图1，图1是本发明实施例提供的一种卡尔曼滤波器的实现方法的流程图，该方法包括如下步骤：

步骤11、获取并初始化系统参数，所述系统参数包括先验信息和后验观测噪声统计量。

卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。本发明实施例提供一种计算速度快、估计精度高的卡尔曼滤波器实现方法。

再介绍本发明实施例提供的卡尔曼滤波器的实现方法之前，首先介绍卡尔曼滤波器的系统方程和观测方程，卡尔曼滤波器的系统方程为：

x_k＝Ax_k-1+Bu_k-1+w_k-1

这个状态方程是根据上一时刻的状态和控制变量来推测此刻的状态，其中，x_k是k时刻的系统状态，x_k-1是k-1时刻的系统状态，u_k-1是k-1时刻对系统的控制量，大多数情况下，没有控制增益，可以认为控制量为0。这个状态方程是根据上一时刻的状态和控制变量来推测此刻的状态，w_k-1是服从高斯分布的噪声，是预测过程的噪声，它对应了x_k中每个分量的噪声，是期望为0，协方差为Q的高斯白噪声w_k-1～N(0,Q),Q即下文的过程激励噪声Q。

观测方程为：

其中，

是随机观测噪声，服从高斯分布，vk～N(0,R)，R即下文的测量噪声R。

卡尔曼滤波算法有两个基本假设:(1)信息过程的足够精确的模型，是由白噪声所激发的线性(也可以是时变的)动态系统；(2)每次的测量信号都包含着附加的白噪声分量。当满足以上假设时，可以应用卡尔曼滤波算法。

卡尔曼滤波算法分为两步:预测和更新

预测：根据上一时刻(k-1时刻)的后验估计值来估计当前时刻(k时刻)的状态，得到k时刻的先验估计值；

更新：使用当前时刻的测量值来更正预测阶段估计值，得到当前时刻的后验估计值。

卡尔曼滤波器可以分为时间更新方程和测量更新方程。时间更新方程(即预测阶段)根据前一时刻的状态估计值推算当前时刻的状态变量先验估计值和误差协方差先验估计值；测量更新方程(即更新阶段)负责将先验估计和新的测量变量结合起来构造改进的后验估计。时间更新方程和测量更新方程也被称为预测方程和校正方程。因此卡尔曼算法是一个递归的预测—校正方法。

卡尔曼滤波器时间更新方程如下：

卡尔曼滤波器状态更新方程如下：

其中，

和

分别表示k时刻和k-1时刻的后验状态估计值，是滤波的结果之一，即更新后的结果，也叫最优估计。

表示k时刻的先验状态估计值，是滤波的中间计算结果，即根据上一时刻(k-1时刻)的最优估计预测的k时刻的结果，是预测方程的结果。

和

分别表示k时刻和k-1时刻的后验估计协方差(即的协方差，表示状态的不确定度)，是滤波的结果之一。

表示k时刻的先验估计协方差(的协方差)，是滤波的中间计算结果。

是状态变量到测量(观测)的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波器为线性关系，它负责将m维的测量值转换到n维，使之符合状态变量的数学形式，是滤波的前提条件之一。

表示测量值(观测值)，是滤波的输入。

K_k表示滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。

A表示状态转移矩阵，实际上是对目标状态转换的一种猜想模型。例如在机动目标跟踪中，状态转移矩阵常常用来对目标的运动建模，其模型可能为匀速直线运动或者匀加速运动。当状态转移矩阵不符合目标的状态转换模型时，滤波会很快发散。

Q表示过程激励噪声协方差(系统过程的协方差)。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号，所以Q的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量，也叫预测模型本身带来的噪声。状态转移协方差矩阵

表示测量噪声协方差。滤波器实际实现时，测量噪声协方差R一般可以观测得到，是滤波器的已知条件。

B表示过程噪声转移矩阵，是将输入转换为状态的矩阵。

表示实际观测和预测观测的残差，和卡尔曼增益一起修正先验 (预测)，得到后验。

本步骤中，首先获取输入噪声，状态转移矩阵A，量测转移矩阵和过程造成转移矩阵B等先验信息，并为初始状态、初始状态协方差矩阵和后验观测噪声统计量赋初值。

步骤12、获取量测信息，根据所述量测信息和后验观测噪声统计量计算有效卡尔曼增益。

在本步骤中，利用卡尔曼时间更新方程和状态更新方程，对获取的量测信息，根据量测信息和后验观测噪声统计量计算有效卡尔曼增益。

步骤13、根据线性观测模型和多元高斯分布的线性性质得到量测的概率密度函数。

在本申请中，首先假设状态、量测和噪声服从高斯分布，且噪声的先验分布已知。利用多元高斯分布线性变换的性质，得到k时刻量测的概率密度函数。

如图2所示，图2是本发明实施例提供的卡尔曼滤波器实现方法中得到量测的概率密度函数的实现方法流程图，根据线性观测模型和多元高斯分布的线性性质得到量测的概率密度函数，一种可能的实施方式是通过以下方式实现：

步骤131、利用先验信息获得线性观测模型

如图3所示，图3是高斯分布线性变换性质的示意图，利用状态和随机观测噪声获得量测值。

步骤132、根据所述线性观测模型和多元高斯分布

的线性性质，得到k时刻以噪声参数θ为条件的量测信息，即

在本步骤中，将

的概率密度函数近似为不确定噪声参数θ为条件的

的概率密度函数，即

步骤133、将

的概率密度函数近似为以不确定噪声参数θ为条件的

的概率密度函数，即

步骤14、根据所述概率密度函数计算通过重要性采样获得的采样点的后验观测噪声分布的期望。

在本步骤中，通过重要性采样方法获得采样点后，根据概率密度函数计算采样点的后验观测噪声分布的期望。

步骤14一种可能的实施方式可以通过以下步骤实现：

步骤141、对新选取的高斯分布进行重要性采样，获得采样点。

如图4所示，图4是本发明实施例提供的卡尔曼滤波器实现方法中利用重要性采样获得采样点的示意图。重要性采样(英语：importance sampling) 是统计学中估计某一分布性质时使用的一种方法。该方法从与原分布不同的另一个分布中采样，而对原先分布的性质进行估计。重要性采样与计算物理学中的伞形采样相关。

图4中纵坐标表示高斯分布，横坐标表示不确定噪声参数，对新选取的高斯分布进行采样，获得采样点{θ₁,θ₂,θ₃,…,θ_n}，其中，n为采样点的数目。

步骤142、根据所述概率密度函数计算所述采样点的后验观测噪声分布的期望。

在本步骤中，根据概率密度函数计算采样点的后验观测噪声分布的期望。

步骤15、根据所有历史时刻后验观测噪声分布的期望对所述后验观测噪声统计量进行更新，获得更新后的后验观测噪声统计量。

在本步骤中，对所有历史时刻获得的后验观测噪声分布的期望进行求平均，获得更新后的后验观测噪声统计量。

步骤16、根据所述有效卡尔曼增益以及更新后的后验观测噪声统计量对状态协方差矩阵进行更新。

在本步骤中，利用卡尔曼滤波器的时间更新方程和状态更新方程，根据有效卡尔曼增益以及更新后的后验观测噪声统计量对状态协方差矩阵进行更新。

步骤17、根据更新后的状态协方差矩阵对状态进行更新，重复执行获取量测信息至对状态协方差矩阵进行更新的步骤，直至状态实现收敛。

在本步骤中，根据更新后的协方差矩阵对状态进行更新，重复执行步骤 12至步骤17，直至状态实现收敛。

本发明实施例还提供一种卡尔曼滤波器的实现装置，该装置用以实现上述实施例公开的方法，如图5所示，图5是本发明另一实施例提供的卡尔曼滤波器实现装置的结构示意图，所述卡尔曼滤波器的实现装置可包括：

初始化模块21，用于获取并初始化系统参数，所述系统参数包括先验信息和后验观测噪声统计量。

卡尔曼增益计算模块22，用于获取量测信息，根据所述量测信息和后验观测噪声统计量计算有效卡尔曼增益。

概率密度函数计算模块23，用于根据线性观测模型和多元高斯分布的线性性质得到量测的概率密度函数。

后验观测噪声分布期望计算模块24，用于根据所述概率密度函数计算通过重要性采样获得的采样点的后验观测噪声分布的期望。

后验观测噪声统计量更新模块25，用于根据所有历史时刻后验观测噪声分布的期望对所述后验观测噪声统计量进行更新，获得更新后的后验观测噪声统计量。

状态协方差矩阵更新模块26，用于根据所述有效卡尔曼增益后更新后的后验观测噪声统计量对状态协方差矩阵进行更新。

迭代模块27，用于根据更新后的状态协方差矩阵对状态进行更新，重复执行获取量测信息至状态信息进行更新的步骤，直至状态实现收敛。

进一步的，所述先验信息可包括过程噪声、状态转移矩阵、量测转移矩阵。

进一步的，所述初始化系统参数可包括为初始状态、初始状态协方差矩阵、后验观测噪声统计量赋初值。

进一步的，所述概率密度函数计算模块23可包括：

线性观测模型获取单元231，用于利用先验信息获得线性观测模型

第一计算单元232，用于根据所述线性观测模型和多元高斯分布

的线性性质，得到k时刻以噪声参数θ为条件的量测信息，即

近似单元233，用于将

的概率密度函数近似为以不确定噪声参数θ为条件的

的概率密度函数，即

其中，

表示k时刻的随机观测噪声，

表示误差协方差矩阵，

表示估计状态。

进一步的，后验观测噪声分布期望计算模块24可包括：

采样单元241，用于对新选取的高斯分布进行重要性采样，获得采样点。

第二计算单元242，用于根据所述概率密度函数计算所述采样点的后验观测噪声分布的期望。

需要说明的是，本发明实施例提供的卡尔曼滤波器实现装置可参照上述实施例公开的卡尔曼滤波器的实现方法进行理解，此处不再赘述。

需要说明的是，本发明实施例中的关节点运动预测装置与上述方法属于相同的发明构思，未在本装置中详述的技术细节可参见前面对方法的相关描述，在此不再赘述。

此外，本发明实施例还提供一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行前面所述的方法。

本发明实施例还提供一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行前面所述的方法。

本领域普通技术人员可以理解上述方法中的全部或部分步骤可通过程序来指令相关硬件(例如处理器)完成，所述程序可以存储于计算机可读存储介质中，如只读存储器、磁盘或光盘等。可选地，上述实施例的全部或部分步骤也可以使用一个或多个集成电路来实现。相应地，上述实施例中的每个模块/单元可以采用硬件的形式实现，例如通过集成电路来实现其相应功能，也可以采用软件功能模块的形式实现，例如通过处理器执行存储于存储器中的程序/指令来实现其相应功能。本发明不限制于任何特定形式的硬件和软件的结合。

虽然本发明所揭露的实施方式如上，但所述的内容仅为便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属领域内的技术人员，在不脱离本发明所揭露的精神和范围的前提下，可以在实施的形式及细节上进行任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。