具体实施方式
下面将详细描述本申请的各个方面的特征和示例性实施例,为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及具体实施例,对本申请进行进一步详细描述。应理解,此处所描述的具体实施例仅意在解释本申请,而不是限定本申请。对于本领域技术人员来说,本申请可以在不需要这些具体细节中的一些细节的情况下实施。下面对实施例的描述仅仅是为了通过示出本申请的示例来提供对本申请更好的理解。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括……”限定的要素,并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。
为了解决现有技术问题,本申请实施例提供了一种滤波器控制方法、装置、设备及计算机存储介质。下面首先对本申请实施例所提供的滤波器控制方法进行介绍。
该滤波器控制方法可以应用在需要使用滤波器对状态参数进行估计的***中。
举例来说,滤波器可以是卡尔曼滤波器,或者是贝叶斯滤波器等可用于状态估计的滤波器。这些滤波器可以获取对状态参数的观测值,结合***对状态参数的预测值,输出状态参数的估计值。从另一个角度来说,该估计值可以认为是对包括有噪声和干扰的观测值进行滤波后得到。
上述的***可以是运动控制***、温度控制***或者湿度控制***等等,此处不做具体限制。
以运动控制***为例,在该***中,状态参数可以是位置、姿态、速度以及加速度中的至少一种;而以温度控制***为例,该***中的状态参数可以是温度等。类似地,在其他类型的***中,通常也具有相应的状态参数。
为了简化描述,下文实施例中,将主要以滤波器为卡尔曼滤波器,***为运动控制***为例进行说明。
在卡尔曼滤波器中,受零均值高斯噪声干扰的离散时间线性动态***可以通过下式进行体现:
其中,x k 为***在k时刻的状态向量,该状态向量通常可以认为是k时刻***的状态参数的真实值;F k-1为状态转移矩阵;x k-1为***在k-1时刻的状态向量;q k-1为***过程噪声,通常来说,q k ~N(0,Q k ),即q k 服从零均值的高斯分布,方差为Q k ;
y k 为k时刻***的观测向量,H k 为k时刻***的观测矩阵,r k 为***的观测噪声,通常来说,r k ~N(0,R k ),即r k 服从零均值的高斯分布,方差为R k 。
式(1)描述的动态***的标准卡尔曼滤波器方程为:
其中,
为***k时刻状态参数的预测值;
m k-1为***k-1时刻状态参数的估计值;
为***k时刻协方差矩阵的预测值;
P k-1为***k-1时刻的协方差矩阵;
K k 为卡尔曼增益;
m k 为***k时刻状态参数的估计值;
P k 为***k时刻协方差矩阵的估计值;
I为相应维数的单位矩阵;上标T表示矩阵或向量的转置。
一般情况下,卡尔曼滤波器在k时刻得到的m k 和P k ,会作为下一时刻的输入。
图1示出了本申请一个实施例提供的滤波器控制方法的流程示意图。如图1所示,该方法包括:
步骤101,在第k个状态估计周期中,在获取到目标状态参数的观测值的情况下,基于观测值确定目标状态参数的估计值;
步骤102,根据估计值,确定第k个状态估计周期对应的误差传递关系,误差传递关系用于指示一个状态估计周期中的第一误差与前一个状态估计周期中的第一误差之间的传递关系,第一误差为目标状态参数的实际值与估计值之间的差值;
步骤103,根据第k个状态估计周期及其前N个状态估计周期对应的误差传递关系,输出第k个状态估计周期中对滤波器的控制策略;
其中,N为自然数,k为大于N+1的整数。
本实施例中,一个状态估计周期可以对应上文中的一个时刻。换而言之,在每一个状态估计周期中,可以获取目标状态参数的观测值,并可以输出目标状态参数的估计值。
一般来说,第k个状态估计周期中,序号k可以是指从滤波器开启或者重置开始计数得到的序号。
这里的目标状态参数,可以根据实际应用场景进行确定。例如:对于运动控制***,目标状态参数可以是位置、姿态、速度以及加速度等运动参数中的至少一项;而对于温度控制***,目标状态参数可以是温度等;对于湿度控制***,目标状态参数可以是湿度等。
结合一个实际应用场景,在车辆、可移动机器人等类型的可移动设备中,上述的目标状态参数可以是运动参数。这些运动参数的观测值可以通过相应的传感器进行采集得到。
例如,位置数据可以是通过基于全球导航卫星***(Global NavigationSatellite System,GNSS)的定位设备进行获取的;姿态数据可以是通过激光雷达结合点云地图、视觉***,或者惯性测量单元(Inertial Measurement Unit,IMU)进行获取的;速度数据可以是通过速度传感器等进行获取的。
当然,在其他的应用场景中,温度的观测值可以是通过温度传感器进行获取的;湿度的观测值可以是通过湿度传感器进行获取的。此处不做一一举例说明。
步骤101中,在获取到目标状态参数的观测值的情况下,可以基于观测值确定目标状态参数的估计值。
结合式(2),观测值可以对应的是y k 这一参数,估计值可以对应m k 。可见,结合观测值与卡尔曼滤波器在上一状态估计周期的输出,可以得到当前状态估计周期对应的目标状态参数的估计值。
在步骤102中,可以根据估计值,确定第k个状态估计周期对应的误差传递关系。其中,总体来说,该误差传递关系可以通过以下方式进行获取:
在第k个状态估计周期中,目标状态参数的真实值为x k ,目标状态参数的估计值为m k 。根据x k 与m k 可以计算得到第一误差,记为ξ k 。其中,第一误差的计算公式可以是如下所示:
容易理解的是,卡尔曼滤波器通常计算的是状态参数的概率分布,估计值可以认为是状态参数最有可能出现的值。换而言之,真实的***状态是确定的,只是通常无法直接获得。因此,一般是计算***状态参数服从的概率分布,并将概率密度最大的参数值认为是***的状态,该概率密度最大的参数值即估计值。
同理,在第k-1个状态估计周期中,也可以得到相应的第一误差,记为ξ k-1。
根据ξ k 与ξ k-1,可以得到第k个状态估计周期对应的误差传递关系。比如,该误差传递关系可以表现为ξ k-1到ξ k 的传递矩阵,或者是ξ k-1的期望值到ξ k 的期望值的传递矩阵等,此处不做具体限定。
一般来说,对于非线性***、***建模有误差,或者是滤波器处于非正常工作状态时,均有可能出现第一误差;当误差传递关系存在较大波动,或者偏离于正常水平时,可能代表卡尔曼滤波器未能正常稳定工作。
因此,在步骤103中,获取第k个状态估计周期及其前N个状态估计周期对应的误差传递关系,来评价卡尔曼滤波器的工作状态,进而输出第k个状态估计周期中对滤波器的控制策略。
N可以是自然数。具体来说,N可以正整数,通过多个状态估计周期对应的多个误差传递关系,来获取误差传递关系的变化趋势,进而可以较为准确地对卡尔曼滤波器的工作状态进行评价。
当然,N也可以是等于0,也就是说,在确定第k个状态估计周期中对滤波器的控制策略,可以仅仅考虑当前k个状态估计周期对应的误差传递关系,从而提高卡尔曼滤波器控制的实时性。
至于对滤波器的控制策略,可以是指示卡尔曼滤波器进入下一个状态估计周期。或者,在上述误差传递关系出现较大波动等情况下,控制卡尔曼滤波器进行重启等等。
本申请实施例提供的滤波器控制方法,可以在一个状态估计周期中根据获取的目标状态参数的观测值,确定目标状态参数的估计值,并基于估计确定该状态估计周期对应的误差传递关系;根据该状态估计周期对应的误差传递关系,或者进一步结合之前状态估计周期对应的误差传递关系,输出该状态估计周期中对滤波器的控制策略。本申请实施例可以通过误差传递关系来评价滤波器的工作状态,基于误差传递关系输出对滤波器的控制策略,有助于提升滤波器工作的稳定性。
可选地,误差传递关系包括目标误差传递矩阵,目标误差传递矩阵为一个状态估计周期中的第一误差的期望与前一个状态估计周期中的第一误差的期望之间的传递矩阵;
根据第k个状态估计周期及其前N个状态估计周期对应的误差传递关系,输出第k个状态估计周期中对滤波器的控制策略,包括:
根据N+1个条件数,输出第k个状态估计周期中对滤波器的控制策略,N+1个条件数为第k个状态估计周期及其前N个状态估计周期对应的目标误差传递矩阵的条件数。
如上文所示的,在第k个状态估计周期中,第一误差可以记为ξ k ,而第一误差的期望可以记为μ k ,也就是说,存在如下关系:
结合式(1)、(2)、(3)以及(4)可以得到:
由于q k-1与r k 均为服从零均值的高斯分布,两者的期望值是等于0的,因此,式(5)可以简化为:
可见,μ k-1与μ k 之间的传递矩阵为(I-K k H k )F k-1。记第k个状态估计周期中,目标误差传递矩阵为A k ,则有:
根据式(6)和(7)可以得到:
若将式(8)看作关于
μ k 的方程,则该方程系数矩阵
的条件数为:
其中,κ表示条件数;|| ||表示求取范数,具体的范数求取方式此处可不做限定,举例来说,可以使用诱导范数等一系列可以量化矩阵对向量在某一测度上的变换能力的范数。
线性***的卡尔曼滤波器的状态估计为一个无偏估计,所以滤波器正常工作时有μ k =0。但是对于非线性***或者***建模有误差以及滤波器处于非正常工作状态时A k 会引入扰动δA k ,μ k 也不再为0。此时具有如下关系:
A k 的扰动δA k 可以等效看作δμ k-1为μ k-1的扰动,即:
结合式(8)与(11),可以得到:
一般来说,可以定义如下通式:
在式(8)、(12)和(13)的基础上可以存在如下关系:
在式(14)的基础上,不等号同侧数据相乘,可以得到如下关系:
在式(8)~(13)的基础上,可以得到如下关系:
其中,sup( )为求取上确界,即求取最小上界。
当
κ(
A k )
=1时,有
sup(
f k )≤1,即当
A k 为一个常数矩阵时,运行卡尔曼滤波器得到的
……的序列为非递增序列,则此时扰动δ
μ k-1不会使δ
μ k ,δ
μ k+1,δ
μ k+2……的序列发散,卡尔曼滤波器不会发散。
当A k 不为常数矩阵时,即***为线性时变***时,卡尔曼滤波器稳定后,如果有κ(A k ),κ(A k+1),κ(A k+2)……的序列收敛到1时,则对应任何一个κ(A k ),均有sup(f k )≤1,此时扰动δμ k-1不会使δμ k ,δμ k+1,δμ k+2……的序列发散,卡尔曼滤波器不会发散的结论依然成立。
相反地,如果κ(A k ),κ(A k+1),κ(A k+2)……的序列不收敛,或者是未收敛到1附近,则说明卡尔曼滤波器运动状态不稳定。
可见,本实施例中,基于目标误差传递矩阵的条件数来输出对滤波器的控制策略,可以充分考虑各种扰动对滤波器工作过程的影响,有助于提高滤波器工作的稳定性。与此同时,对滤波器是否会发散的判断依据的选取比较方便,有助于扩大滤波器控制方法的适用范围。
可选地,根据N+1个条件数,输出第k个状态估计周期中对滤波器的控制策略,包括以下至少一项:
在N+1个条件数收敛于预设数值的情况下,输出第一状态标识;
在N+1个条件数收敛于预设数值以外的数值的情况下,输出第二状态标识;
在N+1个条件数未收敛的情况下,输出第三状态标识;
其中,第一状态标识用于指示滤波器进入下一个状态估计周期,第二状态标识与第三状态标识用于指示滤波器重启。
结合上一实施例的说明,上述的预设数值可以是选择1。
而一般来说,一个稳定的卡尔曼滤波器的κ(A k )序列由于***的建模存在误差,不可能完全收敛到1,而是收敛到1的附近。则可以认为κ(A k )序列收敛到1附近的卡尔曼滤波器是在稳定运行的。因此,预设数值也可以是指1附近的数值。
换而言之,本实施例中,预设数值可以是一些1及其附近数值的集合。
举例来说,该预设数值作为收敛边界β,取值范围可以是β∈[1,o)。其中,o可以为滤波器正常工作时κ(A k )的上确界,该上确界可以是一经验值,根据滤波器的历史工作情况进行选取。
当N+1个状态估计周期中的N+1个条件值收敛于上述的预设数值时,可以输出第一状态标识。第一状态标识可以用于指示滤波器进入下一个状态估计周期。换而言之,在N+1个条件值收敛于上述的预设数值时,可以认为滤波器处于稳定运行的状态,控制其继续工作即可。
容易理解的是,N+1个条件值可以组成上述κ(A k )序列。
在一个示例中,对于κ(A k )序列的收敛性的判断,可以是按照如下方式进行的:
当满足N+1个条件值满足|κ(A k )-β|≤γ时,认为κ(A k )序列是收敛的。其中,如上文所示的,β可以是以预设的收敛边界,且β∈[1,o);o可以为滤波器正常工作时κ(A k )的上确界;γ可以等于0或者约大于0的值,即γ≥0。
当然,在一些可行的实施方式中,上述的收敛性的判断方式,也可以替换成求取N+1个条件数的平均值,或者数值的变化趋势等等。例如,当N+1个条件数的平均值与预设数值之间的差值小于一预设差值时,可以认为N+1个条件数是收敛于预设数值的。再例如,当N+1个条件数依次递减,且第k个状态估计周期得到的条件数与预设数值之间的差值小于一预设差值时,可以认为N+1个条件数是收敛于1的。
另外,当N=0时,可以在第k个状态估计周期得到的条件数与预设数值之间的差值小于一预设差值时,直接认为N+1个条件数是收敛于预设数值的。
当然,在实际应用中,上述N+1个条件数也可能并非是收敛于预设数值的。具体可以至少存在以下两种情况:
一是上述N+1个条件数收敛于预设数值以外的数值。比如,如图2所示,上述预设数值为1,κ(A k )序列初始收敛到1附近,滤波器正常工作。在受到干扰后,κ(A k )序列短暂发散,并稳定收敛到非1附近。此时,***的状态参数的估计值可以表现为如图3所示的,呈一定周期性的稳定震荡。当然,估计值也有可能表现为剧烈无规律的震荡;
二是上述N+1个条件数直接发散。
本实施例中,在N+1个条件数收敛于预设数值以外的数值的情况下,可以输出第二状态标识;而在N+1个条件数未收敛的情况下,输出第三状态标识。
其中,第二状态标识与第三状态标识均可以认为是在滤波器处于非稳定工作状态下输出的,因此,第二状态标识与第三状态标识均可以用于指示滤波器重启。
综上,通过输出状态标识,实际上可以实现对滤波器的控制。输出状态标识,在一定程度上可以认为输出的是对滤波器的控制策略。
值得强调的是,各状态标识对滤波器的控制功能,可以是指状态标识可以直接用于指示滤波器直接进行状态参数的估计,或者进行重启;也可以是在状态标识在被相关控制器读取后,进一步生成对应的控制指令,以控制滤波器进行相应动作。
本实施例中,在N+1个条件数处于不同的收敛状态时,可以输出对应的状态标识,以进一步实现对滤波器的控制,从而有助于有效提高滤波器的工作过程稳定性。
另外,第二状态标识与第三状态标识虽然均可以用于指示滤波器重启;但通过输出不同的状态标识,可以对滤波器在不同情况下出现的不稳定运行状态进行区分,进而也有助于按照状态标识的类型进行相应的报错。
可选地,目标状态参数包括定位参数与非定位参数;
根据N+1个条件数,输出第k个状态估计周期中对滤波器的控制策略之后,方法还包括:
在检测到第二状态标识的情况下,响应于第二状态标识,获取定位信号强度;
在定位信号强度大于或等于预设信号强度的情况下,重置滤波器的定位参数与非定位参数;
在定位信号强度小于预设信号强度的情况下,重置滤波器的非定位参数。
如上文中所举例的,滤波器控制方法可以应用在运动控制***中,例如,可以是具体应用在车辆、机器人等可移动设备中。
以车辆为例,较多的车辆可能需要使用GNSS进行定位。然而,在车辆行驶至某些场景中,例如隧道或者其他GNSS信号较弱,或者无GNSS信号的场景中时,可能会导致定位参数的观测值不准确或者缺失。
相应地,此时会给滤波器的状态参数的估计过程带来较大的扰动,进而影响滤波器的稳定性。
当然,车辆的目标状态参数,除了可以包括定位参数,也可以包括例如姿态、速度等类型的非定位参数。
本实施例中,在检测到第二状态标识的情况下,可以相应第二状态标识,获取定位信号强度。也就是说,本实施例中,在输出第二状态标识之后,可以并非是直接根据第二状态标识进行滤波器的重启。
在定位信号强度大于或等于预设信号强度的情况下,可以对滤波器的定位参数与非定位参数均进行重置,此时在一定程度上可以认为是对滤波器整体的重置,从而有效保证滤波器后续工作过程中的稳定性。
相反地,当定位信号强度小于预设信号强度时,此时若重置定位参数,也可能导致后续无准确的定位参数的观测值的输入(在一些环境,例如隧道中,由于缺少明显的特征也会导致在车辆纵向上定位不准确),导致滤波器无法正常运行。因此,此时,可以仅仅重置例如姿态、速度等非定位参数。
而对于姿态参数,可以基于激光雷达等进行获取;对于速度参数,可以通过速度传感器进行获取。这些非定位参数的观测值的获取,可以不依赖于定位信号。因此,将非定位参数进行重置后,后续也可以获取到非定位参数的准确的观测值,保证滤波器的稳定工作。
可见,本实施例中,通过输出不同的状态标识,结合状态标识与定位信号等其他数据进行滤波器的控制,可以提高对滤波器的控制过程的可选择性。
在一个示例中,当检测到第三状态标识,且在车辆的定位信号较差的情况下,可以直接生成报错指令,以提示滤波器运行出错,且重启滤波器也无法获得比较准确的初始状态。
可选地,根据第k个状态估计周期及其前N个状态估计周期对应的误差传递关系,输出第k个状态估计周期中对滤波器的控制策略,包括:
在k大于M的情况下,根据第k个状态估计周期及其前N个状态估计周期对应的误差传递关系,输出第k个状态估计周期中对滤波器的控制策略,其中,M为大于N+1的整数。
本实施例中,对输出对滤波器的控制策略的状态估计周期进行了限定。换而言之,本实施例中,可以是在滤波器运行预设的状态估计周期后,再根据上述的N+1个状态估计周期对应的误差传递关系,来输出控制策略。
以下结合一个举例,来对本实施例进行说明。
该举例中,若N=2,M=6,则可以在第2~6个状态估计周期中计算目标误差传递矩阵的条件数,并在第7个状态估计周期中,首次使用第5、第6以及第7个状态周期得到条件数输出对滤波器的控制策略。相应地,在第1~4个状态估计周期中,得到的相关输出内容,可以不参与到对滤波器运行稳定性的判断,以及对滤波器的控制策略的输出中。
如上文所示的,在状态估计周期的序号,可以是从滤波器开启或者重置以后开始计数得到的序号。结合图2,在最初的几个状态估计周期中,得到的条件数与1之间的差值较大,可以认为滤波器处于不稳定的工作状态;而在之后的状态估计周期中,得到的条件数位于1的附近,此时可以认为滤波器处于稳定的工作状态。
而本实施例中,在输出控制策略之前,先对状态估计周期的序号k与M进行比较;在k大于M时,在输出控制策略,有助于在度过滤波器初始的不稳定的工作阶段后,再进行控制策略的输出,从而提高滤波器控制过程的准确性与合理性。
在一些举例中,M可以是预设的一数值,可以根据滤波器的历史运行情况进行设定。例如,根据滤波器在历史运行过程中,在启动后到达稳定状态所需的状态估计周期数量,来确定M的值。
当然,在一些可行的实施方式中,也可以不考虑M的值,在k大于N+1,保证了用于判断收敛性的条件数的数量后,即可以进行控制策略的输出。
如图4所示,在一个具体应用例中,上述滤波器控制方法输出状态标识的过程可以包括如下步骤:
步骤401,启动卡尔曼滤波器,运行K个状态估计周期;
这里的K值,与上文实施例中M值对应,均可以是用于保证卡尔曼滤波器稳定运行后,再进行后续的动作。
步骤402,在每一个状态估计周期中,在接收到状态参数的观测值的情况下,根据A k =(I-K k H k )F k-1计算A k ,并进一步计算A k 的条件数κ(A k )。
步骤403,将κ(A k )保存到长度为N′的队列中(N′≥1,为了方便可以取N′=1),当队列长度超过N′后,丢弃最老的数据以保持队列长度始终不大于N′。
步骤404,判断κ(A k )序列的收敛性,如果该序列收敛到1附近,即|κ(A k )-β|≤γ,则可以输出状态标志位flag0。其中,β为预先选取的收敛边界,可以取β∈[1,o);o可以为滤波器正常工作时κ(A k )的上确界;γ可以等于0或者约大于0的值,即γ≥0;
如果κ(A k )序列发散则输出状态标志位flag1;如果κ(A k )序列收敛到非1附近的值则输出状态标志位flag2。
步骤405,获取步骤404中输出的状态标志位,如果状态标志位为flag0则滤波器运行正常继续下一周期的状态估计;如果状态标志位为flag1或者flag2,则可以重启滤波器,从而使得滤波器能够继续稳定运行。
可选地,上述的滤波器控制方法还可以包括:
在第j个状态估计周期中,在获取到目标状态参数的观测值的情况下,基于观测值确定目标状态参数的新息,j为大于1的整数;
根据新息与预设新息阈值条件的匹配关系,输出对应的有效性状态标志,有效性状态标志用于指示第j个状态估计周期中获得的观测值的有效性。
这里的第j个状态估计周期,与上文中的第k个状态估计周期,在序号的确定方式上可以是一致的。使用不同的符号进行区别的原因在于,获取新息与获取误差传递关系的过程可以是独立进行的。
其中,新息v k 可以是指目标状态参数的观测值与预测观测值之间的差值,具体可以通过下式进行表示:
容易理解的是,卡尔曼滤波状态估计器通常计算的是状态参数的概率分布,这就要求观测数据的方差要在一定程度上符合真实状况,也就是说,当观测值产生较大误差时,输出的协方差也应该是增大的,当这种条件不再成立时很容易引起***的不稳定,使***出现震荡甚至发散的问题,因此,本实施例中,可以对于观测值进行有效性的判断来保证卡尔曼滤波器的稳定运行。
具体来说,本实施例中,可以结合新息与预设新息阈值条件来判断在一个状态估计周期中获取的观测值是否有效。
对于预设新息阈值条件,可以是针对新息本身设置的,例如,当新息位于某一数值范围内时,可以认为观测值是有效的;而在新息位于该数值范围之外时,可以认为观测值是无效的。
当然,预设新息阈值条件也可以是针对新息平方,或者是归一化新息平方等参数进行设置的。在判断观测值的有效性的过程中,可以针对新息计算相应的参数后,再与预设新息阈值条件进行比较。
换而言之,本实施例中,观测值是否有效,可以根据新息与预设新息阈值条件的匹配关系进行判断。
而在判断出观测值是否有效后,可以针对该观测值输出对应的有效性状态标志,以指示该观测值是否有效。
举例来说,当新息与预设新息阈值条件匹配时,输出的有效性状态标志可以指示该观测值有效,可以用于第j个状态估计周期的目标状态参数的估计;相反地,如果新息与预设新息阈值条件不匹配,输出的有效性状态标志可以指示该观测值无效,进而可以将该观测值剔除。
可见,本实施例使用新息与预设新息阈值条件来判断一个状态估计周期中获得的观测值的有效性,并针对观测值输出相应的有效性状态标志,有助于保证剔除错误的观测数据,从而防止错误数据对滤波器产生干扰。
在一个示例中,上述的预设新息阈值条件可以是针对归一化新息平方建立的。
归一化新息平方可以记为NIS,其计算方式如下:
其中,
;
为新息方差的理论值,结合式(1)、(2)和(18),
可以通过如下公式进行计算:
一般来说,当NIS的值过大时,表示卡尔曼滤波器获得的观测值与理论上应有的观测值不相符,可以认为是错误的观测数据。
而在本示例中,基于归一化新息平方来判断观测值的有效性,可以设置比较通用的预设新息阈值条件(或者说比较通用的数值范围),有效适应滤波器在不同场合下的应用。
本实施例中,为了更加方便地设置预设新息阈值条件,可以在归一化新息平方的基础上,进一步建立映射规则,对归一化新息平方获取一映射值,并针对该映射值设置一取值范围,来界定观测值的有效性。
具体地,在一个实施方式中,根据新息与预设阈值条件的匹配关系,输出对应的有效性状态标识,包括:
获取新息的归一化新息平方;
根据预设映射规则,获取归一化新息平方的映射值,其中,预设映射规则中,归一化新息平方与映射值正相关;
在映射值小于或等于第一阈值的情况下,输出第四状态标识,第四状态标识用于指示第j个状态估计周期中获得的观测值的有效;
在映射值大于第一阈值的情况下,输出第五状态标识,第五状态标识用于指示第j个状态估计周期中获得的观测值的无效。
举例来说,上述的映射关系,可以是一增函数,使得归一化新息平方与映射值正相关即可(即映射值随着归一化新息平方增大而增大)。
记映射值为ε,则上述增函数可以表示为:
ε=Ψ(NIS) (21)
针对映射值,可以设置一第一阈值,记为
α,该
α大于Ψ(
NIS)的最小值,即
。这里的第一阈值,在一定程度上,可以认为上述预设新息阈值条件的组成部分。
当ε≤α时,可以认为观测值是有效的,可以加入到对目标状态参数的估计中。相反地,当ε>α时,可以认为观测值是无效的,可以进行剔除。
在一个举例中,式(21)可以具体表示为:
。当然,实际应用中其中的各个系数是可以进行调整的,保证式(21)为增函数即可。
在另一些举例中,上述的映射关系,也可以是数值区间对数值的映射关系。例如,一定数值范围的归一化新息平方,可以对应一个映射值等。
如图5所示,在一个具体应用例中,上述滤波器控制方法判断观测值有效性的过程可以包括如下步骤:
步骤501,启动卡尔曼滤波器,运行K个状态估计周期。
一般情况下,卡尔曼滤波器运行K个状态估计周期后,可以进入一比较稳定的工作状态,此时再进一步判断观测数据的有效性,可以提高有效性判断的准确度与合理性。
这里的K值,可以是根据卡尔曼滤波器的历史运行情况取的经验值。
步骤502,步骤501完成后,有观测值输入时计算NIS的值。
步骤503,计算ε=Ψ(NIS)的值,如果ε≤α则输出状态标志位flag3,否则输出状态标志位flag4。
步骤504,获取步骤503中的状态标志位,如果状态标志位为flag3则将观测值加入到滤波器中参与状态估计,如果状态标志位为flag4则剔除该观测值,从而防止错误数据对卡尔曼滤波器的干扰。
结合上文实施例中关于观测值有效性的判断,以及根据条件数判断滤波器稳定性的描述可见,本申请实施例可以实时进行观测数据的有效性判断实现错误观测数据的剔除,实时进行滤波器运行状态的判断必要时进行重启,可大大提高卡尔曼滤波器的稳定性和抗干扰能力。
如图6所示,本申请实施例还提供了一种滤波器控制装置,该装置包括:
第一确定模块601,用于在第k个状态估计周期中,在获取到目标状态参数的观测值的情况下,基于观测值确定目标状态参数的估计值;
第二确定模块602,用于根据估计值,确定第k个状态估计周期对应的误差传递关系,误差传递关系用于指示一个状态估计周期中的第一误差与前一个状态估计周期中的第一误差之间的传递关系,第一误差为目标状态参数的实际值与估计值之间的差值;
第一输出模块603,用于根据第k个状态估计周期及其前N个状态估计周期对应的误差传递关系,输出第k个状态估计周期中对滤波器的控制策略;
其中,N为自然数,k为大于N+1的整数。
可选地,误差传递关系包括目标误差传递矩阵,目标误差传递矩阵为一个状态估计周期中的第一误差的期望与前一个状态估计周期中的第一误差的期望之间的传递矩阵;
相应地,第一输出模块603可具体用于:
根据N+1个条件数,输出第k个状态估计周期中对滤波器的控制策略,N+1个条件数为第k个状态估计周期及其前N个状态估计周期对应的目标误差传递矩阵的条件数。
可选地,第一输出模块603,包括以下至少一项:
第一输出单元,用于在N+1个条件数收敛于预设数值的情况下,输出第一状态标识;
第二输出单元,用于在N+1个条件数收敛于预设数值以外的数值的情况下,输出第二状态标识;
第三输出单元,用于在N+1个条件数未收敛的情况下,输出第三状态标识;
其中,第一状态标识用于指示滤波器进入下一个状态估计周期,第二状态标识与第三状态标识用于指示滤波器重启。
可选地,目标状态参数包括定位参数与非定位参数;
相应地,滤波器控制装置还可以包括:
获取模块,用于在检测到第二状态标识的情况下,响应于第二状态标识,获取定位信号强度;
第一重置模块,用于在定位信号强度大于或等于预设信号强度的情况下,重置滤波器的定位参数与非定位参数;
第二重置模块,用于在定位信号强度小于预设信号强度的情况下,重置滤波器的非定位参数。
可选地,第一输出模块603可具体用于:
在k大于M的情况下,根据第k个状态估计周期及其前N个状态估计周期对应的误差传递关系,输出第k个状态估计周期中对滤波器的控制策略,其中,M为大于N+1的整数。
可选地,滤波器控制装置还可以包括:
第二确定模块,用于在第j个状态估计周期中,在获取到目标状态参数的观测值的情况下,基于观测值确定目标状态参数的新息,j为大于1的整数;
第二输出模块,用于根据新息与预设新息阈值条件的匹配关系,输出对应的有效性状态标志,有效性状态标志用于指示第j个状态估计周期中获得的观测值的有效性。
可选地,第二输出模块,可以包括:
第一获取单元,用于获取新息的归一化新息平方;
第二获取单元,用于根据预设映射规则,获取归一化新息平方的映射值,其中,预设映射规则中,归一化新息平方与映射值正相关;
第四输出单元,用于在映射值小于或等于第一阈值的情况下,输出第四状态标识,第四状态标识用于指示第j个状态估计周期中获得的观测值的有效;
第五输出单元,用于在映射值大于第一阈值的情况下,输出第五状态标识,第五状态标识用于指示第j个状态估计周期中获得的观测值的无效。
需要说明的是,该滤波器控制装置是与上述滤波器控制方法对应的装置,上述方法实施例中所有实现方式均适用于该装置的实施例中,也能达到相同的技术效果。
图7示出了本申请实施例提供的电子设备的硬件结构示意图。
在电子设备可以包括处理器701以及存储有计算机程序指令的存储器702。
具体地,上述处理器701可以包括中央处理器(CPU),或者特定集成电路(Application Specific Integrated Circuit ,ASIC),或者可以被配置成实施本申请实施例的一个或多个集成电路。
存储器702可以包括用于数据或指令的大容量存储器。举例来说而非限制,存储器702可包括硬盘驱动器(Hard Disk Drive,HDD)、软盘驱动器、闪存、光盘、磁光盘、磁带或通用串行总线(Universal Serial Bus,USB)驱动器或者两个或更多个以上这些的组合。在合适的情况下,存储器702可包括可移除或不可移除(或固定)的介质。在合适的情况下,存储器702可在综合网关容灾设备的内部或外部。在特定实施例中,存储器702是非易失性固态存储器。
存储器可包括只读存储器(ROM),随机存取存储器(RAM),磁盘存储介质设备,光存储介质设备,闪存设备,电气、光学或其他物理/有形的存储器存储设备。因此,通常,存储器包括一个或多个编码有包括计算机可执行指令的软件的有形(非暂态)计算机可读存储介质(例如,存储器设备),并且当该软件被执行(例如,由一个或多个处理器)时,其可操作来执行参考根据本公开的方法所描述的操作。
处理器701通过读取并执行存储器702中存储的计算机程序指令,以实现上述实施例中的任意一种滤波器控制方法。
在一个示例中,电子设备还可包括通信接口703和总线704。其中,如图7所示,处理器701、存储器702、通信接口703通过总线704连接并完成相互间的通信。
通信接口703,主要用于实现本申请实施例中各模块、装置、单元和/或设备之间的通信。
总线704包括硬件、软件或两者,将在线数据流量计费设备的部件彼此耦接在一起。举例来说而非限制,总线可包括加速图形端口(AGP)或其他图形总线、增强工业标准架构(EISA)总线、前端总线(FSB)、超传输(HT)互连、工业标准架构(ISA)总线、无限带宽互连、低引脚数(LPC)总线、存储器总线、微信道架构(MCA)总线、***组件互连(PCI)总线、PCI-Express(PCI-X)总线、串行高级技术附件(SATA)总线、视频电子标准协会局部(VLB)总线或其他合适的总线或者两个或更多个以上这些的组合。在合适的情况下,总线704可包括一个或多个总线。尽管本申请实施例描述和示出了特定的总线,但本申请考虑任何合适的总线或互连。
另外,结合上述实施例中的滤波器控制方法,本申请实施例可提供一种计算机存储介质来实现。该计算机存储介质上存储有计算机程序指令;该计算机程序指令被处理器执行时实现上述实施例中的任意一种滤波器控制方法。
需要明确的是,本申请并不局限于上文所描述并在图中示出的特定配置和处理。为了简明起见,这里省略了对已知方法的详细描述。在上述实施例中,描述和示出了若干具体的步骤作为示例。但是,本申请的方法过程并不限于所描述和示出的具体步骤,本领域的技术人员可以在领会本申请的精神后,作出各种改变、修改和添加,或者改变步骤之间的顺序。
以上的结构框图中所示的功能块可以实现为硬件、软件、固件或者它们的组合。当以硬件方式实现时,其可以例如是电子电路、专用集成电路(ASIC)、适当的固件、插件、功能卡等等。当以软件方式实现时,本申请的元素是被用于执行所需任务的程序或者代码段。程序或者代码段可以存储在机器可读介质中,或者通过载波中携带的数据信号在传输介质或者通信链路上传送。“机器可读介质”可以包括能够存储或传输信息的任何介质。机器可读介质的例子包括电子电路、半导体存储器设备、ROM、闪存、可擦除ROM(EROM)、软盘、CD-ROM、光盘、硬盘、光纤介质、射频(RF)链路,等等。代码段可以经由诸如因特网、内联网等的计算机网络被下载。
还需要说明的是,本申请中提及的示例性实施例,基于一系列的步骤或者装置描述一些方法或***。但是,本申请不局限于上述步骤的顺序,也就是说,可以按照实施例中提及的顺序执行步骤,也可以不同于实施例中的顺序,或者若干步骤同时执行。
上面参考根据本公开的实施例的方法、装置(***)和计算机程序产品的流程图和/或框图描述了本公开的各方面。应当理解,流程图和/或框图中的每个方框以及流程图和/或框图中各方框的组合可以由计算机程序指令实现。这些计算机程序指令可被提供给通用计算机、专用计算机、或其它可编程数据处理装置的处理器,以产生一种机器,使得经由计算机或其它可编程数据处理装置的处理器执行的这些指令使能对流程图和/或框图的一个或多个方框中指定的功能/动作的实现。这种处理器可以是但不限于是通用处理器、专用处理器、特殊应用处理器或者现场可编程逻辑电路。还可理解,框图和/或流程图中的每个方框以及框图和/或流程图中的方框的组合,也可以由执行指定的功能或动作的专用硬件来实现,或可由专用硬件和计算机指令的组合来实现。
以上,仅为本申请的具体实施方式,所属领域的技术人员可以清楚地了解到,为了描述的方便和简洁,上述描述的***、模块和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。应理解,本申请的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本申请揭露的技术范围内,可轻易想到各种等效的修改或替换,这些修改或替换都应涵盖在本申请的保护范围之内。