基于变分贝叶斯的分布协同非线性系统状态估计方法 |
|||||||
申请号 | CN202210088496.7 | 申请日 | 2022-01-25 | 公开(公告)号 | CN114567288B | 公开(公告)日 | 2024-04-26 |
申请人 | 河南大学; | 发明人 | 金勇; 杨诗博; 杨琳琳; 贾浩乾; 张子寒; 毋嘉伟; | ||||
摘要 | 本 发明 公开了一种基于变分贝叶斯的分布协同非线性系统状态估计方法,通过以变分贝叶斯为 基础 ,针对分布协同非线性目标 跟踪 系统过程噪声时变和观测噪声具有随机异常值的情况,选择IW分布和student’t分布作为目标状态一步预测协方差的先验分布和量测的分布,通过定点 迭代 的方法在各局部 滤波器 中求解目标状态和噪声参数的近似后验分布,采用CI融合方法对各局部滤波器的目标状态估计加权融合得到全局最优估计,最后再将全局最优估计反馈给各局部滤波器,提高了滤波器的估计 精度 和 稳定性 。 | ||||||
权利要求 | 1.一种基于变分贝叶斯的分布协同非线性系统状态估计方法,其特征在于:包括以下步骤: |
||||||
说明书全文 | 基于变分贝叶斯的分布协同非线性系统状态估计方法技术领域背景技术[0002] 无迹卡尔曼滤波算法(Unscented Kalman Filter,UKF)使用无迹变换来处理状态与协方差的非线性传递问题,用一系列确定sigma点来逼近状态的后验概率密度,具有很高的精度和稳定性,传统的UKF算法在系统噪声统计量已知且固定不变的非线性系统状态估计问题中能够取得较好的结果。但是在实际场景下,由于环境变化等原因,系统过程噪声统计量一般未知并且随着时间变化;由于外界扰动、传感器故障等原因,量测噪声异常值会随机出现;使用错误的噪声统计量和异常量测值对系统状态进行估计会导致估计结果变差,甚至引起滤波器发散。同时,受传感器精度影响,单传感器状态跟踪系统滤波精度有待提高。 [0003] 在系统过程噪声时变、量测噪声存在随机异常值的情况下,要想对目标状态进行准确地估计,就要对时变的过程噪声和量测异常值进行估计,但是时变过程噪声与具有异常值的量测的概率分布难以直接得到,因此采用变分贝叶斯(Variational Bayesian,VB)近似方法来解决这个问题。通过对时变的过程噪声和异常量测值选取合适的先验分布,采用定点迭代的方法求取待估计的目标状态和噪声参数的近似后验分布,从而得到目标的状态估计。 [0004] 针对单传感器状态跟踪系统滤波精度不足的问题,引入协方差交叉(Covariance intersection,CI)融合算法,CI融合算法是一种经典的分布式融合算法,该算法能够对各局部传感器的估计结果进行加权融合得到全局最优估计,融合中心得到全局最优估计后,再将全局最优估计反馈给各局部滤波器,因此协方差交叉融合算法具有很高的估计精度,适合于传感器数量较少的分布式目标跟踪系统中。 发明内容[0005] 本发明的目的是提供一种基于变分贝叶斯的分布协同非线性系统状态估计方法,能够在系统过程噪声时变、量测噪声存在随机异常值情况下对目标状态进行准确地估计。 [0006] 本发明采用的关键技术为:利用UKF处理非线性系统状态估计问题;利用变分贝叶斯方法解决了过程噪声时变和量测噪声具有随机异常值问题;利用定点迭代方法解决了目标状态和噪声参数相互耦合无法获取解析解的问题;利用CI融合方法解决了单传感器目标跟踪系统滤波精度不足问题。 [0007] 一种基于变分贝叶斯的分布协同非线性系统状态估计方法,包括以下几个步骤: [0008] 1、建立分布协同非线性目标状态跟踪系统的动态模型; [0009] 所述步骤1具体包括以下步骤: [0010] 1.1、系统的动态空间模型为: [0011] xk=f(xk‑1)+wk‑1; [0012] zk,l=hl(xk)+vk,l l=1,2,...s; [0013] 其中,k是离散时刻,xk、xk‑1分别是k时刻和k‑1时刻的状态向量,它们是n维的变量,zk,l是k时刻第l个传感器的量测向量,它是一个m维的变量,f(xk‑1)是状态转移函数,hl(xk)是第l个传感器的量测函数;wk‑1是k‑1时刻到k时刻的零均值时变的过程噪声向量,它的期望协方差矩阵为Qk‑1;vk,l是k时刻第l个传感器具有随机异常值的量测噪声向量,它的期望协方差矩阵为Rk,l;任意时刻的wk,vk,l和初值状态x0均互不相关。 [0014] 2、在分布式非线性目标状态跟踪系统中,在k时刻,对于第l个局部滤波器(l=1,2,…,s),输入k‑1时刻目标状态估计及其协方差及滤波器参数; [0015] 所述步骤2具体包括以下步骤: [0016] 2.1、输入:k‑1时刻目标状态估计 及其对应的协方差Pk‑1|k‑1,l,k‑1时刻的名义过程噪声协方差阵 k时刻该滤波器对应的量测噪声协方差阵Rk,l,k时刻该滤波器接收到的量测zk,l,调谐参数τl,student’t分布的自由度参数νl,以及变分迭代次数Nm;其中:k‑1时刻的名义过程噪声协方差阵 是由于过程噪声统计量未知而根据经验选定k时刻的名义过程噪声协方差;τl的作用是协调模型先验信息和量测修正信息的权重。 [0017] 3、在第l个局部滤波器中,通过UKF算法求k时刻目标状态一步预测 及其对应的协方差矩阵Pk|k‑1,l; [0018] 所述步骤3具体包括以下步骤: [0019] 3‑1、通过对第l个滤波器中k‑1时刻目标状态估计 进行无迹变换产生2n+1个sigma点: [0020] [0021] [0022] [0023] 其中:n表示 的维度, 表示通过无迹变换产生的第j个sigma点, 表示第j个sigma点的权值, 表示第j个sigma点对应协方差阵的权值, 表示矩阵P方根 2 的第j列,λ=α (n+κ)‑n用来降低总的预测误差,α控制采样点的分布状态,κ的选取应保证(n+λ)P是半正定矩阵,一般取值0,β≥0合并方程中高阶项动差。 [0024] 3‑2、求状态预测及其对应的协方差阵: [0025] [0026] [0027] [0028] 其中: 是由于过程噪声统计量未知而根据经验选定的名义过程噪声协方差;表示第j个sigma点的权值, 表示第j个sigma点对应协方差阵的权值。 [0029] 4、选择逆威沙特(Inverse Wishart,IW)分布和student’t分布作为一步预测协方差的先验分布和量测的分布,并求取IW先验参数: [0030] 一步预测协方差的先验分布: [0031] 量测的分布:p(zk,l|xk)=St(zk,l;hl(xk),Rk,l,vl) [0032] 其中, 表示自由度参数为 和逆尺度参数为 的IW分布。St(zk,l;hl(xk),Rk,l,vl)表示均值为hl(xk),尺度矩阵为Rk,l,自由度参数为v的student’t分布。 [0033] 所述步骤4具体包括以下步骤: [0034] 4‑1、选择IW分布作为一步预测协方差的先验分布: [0035] [0036] 其中,IW分布的自由度参数 逆尺度参数 通过如下方法得到: [0037] 对于一个IW分布:A~IW(A;t,T),其期望可以写为:E[A‑1]=(t‑n‑1)Τ‑1,t≥n+1,其中n是t的维度。因此,步骤3‑2中的状态一步预测协方差Pk|k‑1,l还可以表示为: [0038] [0039] 令: 则: 其中,nx是状态量x的维度,τl≥0是调谐参数,它的选取根据具体的情况而定。 [0040] 4‑2、使用student’t分布作为量测的分布: [0041] p(zk,l|xk)=St(zk,l;hl(xk),Rk,l,vl) [0042] St(zk,l;hl(xk),Rk,l,vl)表示均值为hl(xk),尺度矩阵为Rk,l,自由度参数为v的student’t分布。各滤波器对目标状态估计独立进行,在各滤波器中,假设: [0043] p(zk,l|xk,l)=St(zk,l;hl(xk,l),Rk,l,vl)≈p(zk,l|xk)=St(zk,l;hl(xk),Rk,l,vl),由于student’t分布的概率密度函数的闭合解难以求得,引入一个辅助随机变量λk,l,量测的概率密度函数可以写为: [0044] [0046] p(zk,l|xk,l,λk,l)=N(zk,l;hl(xk,l),Rk,l/λk,l) [0047] 5、变分迭代初始化,根据步骤4得到IW先验参数tk|k‑1,l,Tk|k‑1,l;k时刻第l个局部滤波器中目标状态及协方差阵的迭代初值设置为: student’t分布的辅助变量初值选取为 [0048] 6、变分迭代:在k时刻、第i次变分迭代中(i=1,2,…,Nm),分别求取目标状态(i) (i)一步预测协方差Pk|k‑1,l和辅助变量λk,l的近似后验分布q (xk|k,l),q (Pk|k‑1,l)和q(i) (λk,l); [0049] 所述步骤6具体包括以下步骤: [0050] 6‑1、固定k时刻第i‑1次迭代状态估计的近似后验概率分布q(i‑1)(xk,l),将第i次迭代的状态一步预测协方差Pk|k‑1,l的近似后验概率分布更新为 其中:自由度参数 逆尺度参数 [0051] k时刻第i次迭代一步预测协方差的逆矩阵期望为第i次迭代一步预测协方差可以表示为: [0052] 6‑2、固定k时刻第i‑1次迭代状态估计的近似后验概率分布q(i‑1)(xk,l),将第i次迭代的辅助变量λk,l的近似后验概率分布更新为 [0053] 其中:形状参数 逆尺度参数 [0054] 是按照步骤3‑1的方法对进行无迹变换得到的第j个sigma点, 是对应的权值。 [0055] 辅助变量期望 第i次迭代中辅助变量值表示为:第i次迭代修正量测噪声协方差可以表示为: [0056] 6‑3、固定k时刻第i次迭代状态一步预测协方差Pk|k‑1,l的近似后验概率分布q(i)(i)(Pk|k‑1,l)、第i次迭代辅助变量λk,l的近似后验概率分布q (λk,l),将第i次迭代的对目标状态估计xk,l的近似后验概率分布更新为 其中: 分别表示 第l个滤波器在k时刻第i次迭代中的目标状态估计及其对应的协方差。 [0057] 按照步骤3‑1的方法对状态一步预测 和第i次迭代得到的状态一步预测协方差 进行无迹变换重新获得状态一步预测sigma点以及量测预测 [0058] [0059] [0060] 其中:n表示 的维度, 表示通过无迹变换产生的第j个sigma点, 表示矩阵P方根的第j列, 表示对第j个sigma点的量测预测, 表示第j个sigma点对应的权值, 表示第l个滤波器第k时刻的量测预测。 [0062] [0063] [0064] [0065] [0066] [0067] 7、判断当前迭代次数i是否达到预设的最大变分迭代次数Nm,若是,则执行下一步;若不是,则i=i+1,并重新执行步骤6; [0068] 8、对k时刻各局部滤波器的状态估计按照CI融合算法进行加权融合,再将融合结果反馈给各局部滤波器作为下一时刻的先验; [0069] 所述步骤8具体包括以下步骤: [0070] 8‑1、局部滤波器状态估计的加权融合: [0071] 全局状态估计协方差: [0072] 全局状态估计: [0073] 8‑2、将k时刻全局估计及其协方差按照一定的准则反馈给各局部滤波器: [0074] 状态估计反馈: [0075] 状态估计协方差反馈: 其中,αk,l为反馈权重系数,它们随着各个局部滤波器协方差的变化而变化,αk,l满足以下条件: [0076] αk,1+αk,2+...+αk,l=1 [0077] [0078] 其中:||·||F表示表示Frobenius范数,即对于任意矩阵A: [0079] [0080] 9、输出第k时刻对目标状态的全局状态估计及其协方差: [0081] [0082] 10、判断是否达到预设仿真时长,若不是,则k=k+1,重新执行步骤2;若是,结束执行; [0083] 本发明与现有技术相比,具有以下优点: [0084] (1)引入变分贝叶斯方法,选择用IW分布和student’t分布分别作为目标状态一步预测协方差的先验分布和量测的分布,解决了过程噪声时变和量测噪声具有随机异常值问题; [0085] (2)引入定点迭代方法,能够求得目标状态和噪声参数的近似后验概率分布,有效解决了目标状态和噪声参数相互耦合无法获取解析解的问题; [0087] 图1为本发明的流程图。 [0088] 图2为本发明所述CI融合算法的结构图。 具体实施方式[0089] 下面结合实施例及附图对本发明作进一步详细的描述,但本发明的实施方式不限于此。 [0090] 如图1流程图所示所示,本实施例提供一种基于变分贝叶斯的分布协同非线性系统状态估计方法,包括以下步骤: [0091] 1、建立分布协同非线性目标状态跟踪系统的动态模型; [0092] 2、在分布式非线性目标状态跟踪系统中,在k时刻,对于第l个局部滤波器(l=1,2,…,s),s为滤波器的个数,输入k‑1时刻目标状态估计及其协方差及滤波器参数; [0093] 3、在第l个局部滤波器中通过UKF算法求目标状态一步预测 及其对应的协方差矩阵Pk|k‑1,l; [0094] 4、选择逆威沙特(Inverse Wishart,IW)分布作为一步预测协方差的先验分布,并求取IW先验参数: [0095] 一步预测协方差的先验分布: [0096] 选择student’t分布作为量测的分布: [0097] 量测的分布:p(zk,l|xk)=St(zk,l;hl(xk),Rk,l,vl) [0098] 其中, 表示自由度参数为 和逆尺度参数为 的IW分布。St(zk,l;hl(xk),Rk,l,vl)表示均值为hl(xk),尺度矩阵为Rk,l,自由度参数为v的student’t分布。 [0099] 5、变分迭代初始化,根据步骤4得到IW先验参数tk|k‑1,l,Tk|k‑1,l;第l个局部滤波器中目标状态及协方差阵的迭代初值设置为: student’t分布的辅助变量初值选取为 [0100] 6、变分迭代:在k时刻、第i次变分迭代中(i=1,2,…,Nm),分别求取目标状态(i) (i)的近似后验分布q (xk|k,l)、一步预测协方差Pk|k‑1,l的近似后验分布q (Pk|k‑1,l)和辅助变(i) 量λk,l的近似后验分布q (λk,l); [0101] 7、判断当前迭代次数i是否达到预设的次数Nm,若是,则执行下一步;若不是,则i=i+1,并返回执行步骤6; [0102] 8、对各局部滤波器的状态估计按照CI融合算法进行加权融合,再将融合后的全局状态结果反馈给各局部滤波器作为下一时刻的先验; [0103] 9、输出第k时刻对目标状态的全局状态估计 及其协方差Pk|k,M; [0104] 10、判断是否达到预设仿真时长,若不是,则k=k+1,重新执行步骤2;若是,结束执行; [0105] 其中,所述步骤1具体包括以下步骤: [0106] 1.1、系统的动态空间模型为: [0107] xk=f(xk‑1)+wk‑1; [0108] zk,l=hl(xk)+vk,l l=1,2,...s; [0109] 其中,k是离散时刻,xk、xk‑1分别是k时刻和k‑1时刻的状态向量,它们是n维的变量,zk,l是k时刻第l个传感器的量测向量,它是一个m维的变量,f(xk‑1)是状态转移函数,hl(xk)是第l个传感器的量测函数;wk‑1是k‑1时刻到k时刻的零均值时变的过程噪声向量,它的期望协方差矩阵为Qk‑1;vk,l是k时刻第l个传感器具有随机异常值的量测噪声向量,它的期望协方差矩阵为Rk,l;任意时刻的wk,vk,l和初值状态x0均互不相关。 [0110] 其中,所述步骤2具体包括以下步骤: [0111] 2.1、输入:k‑1时刻目标状态估计 及其对应的协方差Pk‑1|k‑1,l,k‑1时刻的名义过程噪声协方差阵 k时刻该滤波器对应的量测噪声协方差阵Rk,l,k时刻该滤波器接收到的量测zk,l,调谐参数τl,student’t分布的自由度参数νl,以及变分迭代次数Nm;其中:k‑1时刻的名义过程噪声协方差阵 是由于过程噪声统计量未知而根据经验选定k时刻的名义过程噪声协方差;τl在算法中协调模型先验信息和量测修正信息的权重。 [0112] 其中,所述步骤3具体包括以下步骤: [0113] 3‑1、通过对第l个滤波器中k‑1时刻目标状态估计 进行无迹变换产生2n+1个sigma点: [0114] [0115] [0116] [0117] 其中:n表示 的维度, 表示通过无迹变换产生的第j个sigma点, 表示第j个sigma点的权值, 表示第j个sigma点对应协方差阵的权值, 表示矩阵P方 2 根的第j列,λ=α (n+κ)‑n用来降低总的预测误差,α控制采样点的分布状态,κ的选取应保证(n+λ)P是半正定矩阵,一般取值0,β≥0合并方程中高阶项动差。 [0118] 3‑2、求状态预测及其对应的协方差阵: [0119] [0120] [0121] [0122] 其中: 是由于过程噪声统计量未知而根据经验选定的名义过程噪声协方差;表示第j个sigma点的权值, 表示第j个sigma点对应协方差阵的权值。 [0123] 其中,所述步骤4具体包括以下步骤: [0124] 4‑1、选择IW分布作为一步预测协方差的先验分布: [0125] [0126] 其中,IW分布的自由度参数 逆尺度参数 通过如下方法得到: [0127] 对于对于一个IW分布:A~IW(A;t,T),其期望可以写为:E[A‑1]=(t‑n‑1)Τ‑1,t≥n+1,其中n是t的维度。因此,步骤3‑2中的状态一步预测协方差Pk|k‑1,l还可以表示为: [0128] [0129] 令: 则: 其中,nx是状态量x的维度,τl≥0是调谐参数,它的选取根据具体的情况而定。 [0130] 4‑2、使用student’t分布作为量测的分布: [0131] p(zk,l|xk)=St(zk,l;hl(xk),Rk,l,vl) [0132] St(zk,l;hl(xk),Rk,l,vl)表示均值为hl(xk),尺度矩阵为Rk,l,自由度参数为v的student’t分布。各滤波器对目标状态估计独立进行,在各滤波器中,假设: [0133] p(zk,l|xk,l)=St(zk,l;hl(xk,l),Rk,l,vl)≈p(zk,l|xk)=St(zk,l;hl(xk),Rk,l,vl),由于student’t分布的概率密度函数的闭合解难以求得,引入一个辅助随机变量λk,l,量测的概率密度函数可以写为: [0134] [0135] 其中, 表示形状参数为 和逆尺度参数为 的伽马分布。α和β分别为伽马分布的形状参数和逆尺度参数。根据上一个式子,量测的概率密度函数最终可以表示为如下的分层高斯形式: [0136] p(zk,l|xk,l,λk,l)=N(zk,l;hl(xk,l),Rk,l/λk,l) [0137] 其中,所述步骤6具体包括以下步骤: [0138] 6‑1、固定k时刻第i次迭代状态估计的近似后验概率分布q(i‑1)(xk,l),将第i次迭代的状态一步预测协方差Pk|k‑1,l的近似后验概率分布更新为其中:自由度参数 逆尺度参数 [0139] k时刻第i次迭代一步预测协方差的逆矩阵期望为第i次迭代一步预测协方差可以表示为: [0140] 6‑2、固定k时刻第i‑1次迭代状态估计的近似后验概率分布q(i‑1)(xk,l),将第i次迭代的辅助变量λk,l的近似后验概率分布更新为 [0141] 其中:形状参数 逆尺度参数 [0142] 是按照步骤3‑1的方法对进行无迹变换得到的第j个sigma点, 是对应的权值。 [0143] 辅助变量期望 第i次迭代中辅助变量值表示为:第i次迭代修正量测噪声协方差可以表示为: [0144] 6‑3、固定k时刻第i次迭代状态一步预测协方差Pk|k‑1,l的近似后验概率分布q(i)(i)(Pk|k‑1,l)、第i次迭代辅助变量λk,l的近似后验概率分布q (λk,l),将第i次迭代的对目标状态估计xk,l的近似后验概率分布更新为 其中: 分别表 示第l个滤波器在k时刻第i次迭代中的目标状态估计及其对应的协方差。 [0145] 按照步骤3‑1的方法对状态一步预测 和第i次迭代得到的状态一步预测协方差 进行无迹变换重新获得状态一步预测sigma点以及量测预测 [0146] [0147] [0148] 其中:n表示 的维度, 表示通过无迹变换产生的第j个sigma点, 表示矩阵P方根的第j列, 表示对第j个sigma点的量测预测, 表示第j个sigma点对应的权值, 表示第l个滤波器第k时刻的量测预测。 [0149] 在UKF框架下求得 [0150] [0151] [0152] [0153] [0154] [0155] 其中,所述步骤8具体包括以下步骤: [0156] 8‑1、k时刻局部滤波器状态估计的加权融合: [0157] 全局状态估计协方差: [0158] 全局状态估计: [0159] 8‑2、将k时刻全局估计及其协方差按照一定的准则反馈给各局部滤波器: [0160] 状态估计反馈: [0161] 状态估计协方差反馈: 其中,αk,l为反馈权重系数,它们随着各个局部滤波器协方差的变化而变化,αk,l满足以下条件: [0162] αk,1+αk,2+...+αk,l=1 [0163] [0164] 其中:||·||F表示表示Frobenius范数,即对于任意矩阵A: [0165] [0166] 本发明以变分贝叶斯为基础,针对分布协同非线性目标跟踪系统过程噪声时变和观测噪声具有随机异常值的情况,选择IW分布和student’t分布作为目标状态一步预测协方差的先验分布和量测的分布,通过定点迭代的方法在各局部滤波器中求解目标状态和噪声参数的近似后验分布,采用CI融合方法对各局部滤波器的目标状态估计加权融合得到全局最优估计,最后再将全局最优估计反馈给各局部滤波器,提高了滤波器的估计精度和稳定性。 |