基于非线性未知随机偏差的两阶段容积卡尔曼滤波方法
【专利摘要】本发明涉及一种基于非线性未知随机偏差的二阶段容积卡尔曼滤波方法。本发明对于不含未知偏差的部分，算出一步预测值、一步预测方差、一步预测协方差。从而算出增益阵、估计值、估计误差方差。对于含未知偏差的部分，算出一步偏差估计、一步偏差估计方差、一步偏差估计协方差。再算出偏差增益阵、偏差对于状态变量的一步增益阵和估计增益阵。通过两个部分算出来的值，算出含未知偏差非线性系统的一步预测值，一步预测误差方差，估计值和估计误差方差。在完成滤波的同时，避免了对计算机来说大的计算量和维度灾难。
【专利说明】基于非线性未知随机偏差的两阶段容积卡尔曼滤波方法
【技术领域】
[0001] 本发明属于非线性系统的滤波领域，特别涉及一种处理含有未知随机变量非线性 系统的两阶容积卡尔曼滤波方法。
【背景技术】
[0002] 非线性滤波是信号处理、目标跟踪和控制领域方面的热门话题之一，特别是，在卡 尔曼滤波框架下的非线性滤波研究仍然是一个很火的问题，在近年来受到了越来越多的关 注和研究。。
[0003] 卡尔曼滤波器"为线性高斯系统中最小均方差下的位置状态"提供了有效的解决 方法。然而真实世界中，系统总是受到非线性和非高斯两方面的困扰。想要获得关于状态估 计良好的办法是很难的。扩展卡尔曼于是受到了广泛的使用，无踪卡尔曼是另外一种用样 本来解决非线性分布的方法。不幸的是上述提到的非线性滤波器都有发散和维度灾难的问 题。一个更为精确的容积卡尔曼应运而生。容积卡尔曼滤波的核心是球型射线容积法则。 它可以用来解决高斯非线性滤波问题，而不需要很大的计算量。
[0004] 标准的求容积卡尔曼滤波器要求精确的系统模型和精确的随机指标信息，在实际 中模型通常包括未知定量和未知偏差。在处理含有位置偏差的动态系统的状态估计时，将 偏差视为一部分系统状态，并且像系统一样估计偏差。这就引出了增广状态卡尔曼滤波，它 的应用却也存在相对大的计算量的问题。
【发明内容】
[0005] 本发明涉及一种处理还有未知偏差非线性系统的两阶段容积卡尔曼滤波器。该方 法将含有位置偏差的非线性系统看作不含位置偏差估计和含有位置偏差估计两部分。从而 在获得良好非线性滤波的情况下，避免了增广卡尔曼滤波所带来的计算量大的问题。对于 不含未知偏差的部分，算出一步预测值、一步预测方差、一步预测协方差。从而算出增益阵、 估计值、估计误差方差。对于含未知偏差的郃分，算出一步偏差估计、一步偏差估计方差、一 步偏差估计协方差。再算出偏差增益阵、偏差对于状态变量的一步增益阵和估计增益阵。 通过两个部分算出来的值，算出含未知偏差非线性系统的一步预测值，一步预测误差方差， 估计值和估计误差方差。在完成滤波的同时，避免了对计算机来说大的计算量和维度灾难。 本发明的具体步骤如下：
[0006] 步骤1.计算不含有未知偏差部分的无偏差估计一步预测，一步预测误差方 差Pk'k-iP:，k\k-i 一步预测误差协方差峨-1。
[0007] 步骤2.计算不含有未知偏差部分的无偏差估计增益阵估计值毛|和估计误差 方差阵Pk| k。
[0008] 步骤3.计算含有未知偏差部分的，未知偏差一步预测buH偏差一步预测方差 偏差预测误差协方差雄-丨，和< 以及喷。
[0009] 步骤4.计算含有未知偏差部分的，偏差增益阵I〗和偏差估计误差方差阵以 及偏差一步增益UkiH和偏差增益。
[0010] 步骤5.根据以上四步计算未知偏差非线性系统的一步预测方差和一步预测 误差协方差/和最终目标状态的最优线性估计及其误差协方差。
[0011] 本发明有益效果：使用两阶段容积卡尔曼方法不仅能有效处理还有未知偏差的非 线性系统，还有效避免了增广状态卡尔曼在处理同类问题时所产生的维度灾难和计算机计 算量过大的问题。
【专利附图】
【附图说明】
[0012] 图1为两阶段容积卡尔曼滤波的流程图。
【具体实施方式】
[0013] 下面首先为跟踪目标的运动状态建立模型，其次给出两阶段容积强滤波器的滤波 公式，将其分为无偏差估计和有偏差估计两个步骤，下面结合图1详细介绍本发明的实施 过程。
[0014] 1系统建模
[0015] 1. 1给出如下非线性系统动态模型
[0016] xk = fk-iUk-i)+^Kk-i
[0017] zk = hk (xk) +vk
[0018] 其中k彡1是时刻指数，xke RnX1表示系统状态（RnX1为nXl维列向量全集）， zkeRnX1是测量值列向量，f^J·)以及hk(·)都是可微函数。初始状态&服从元为均值， PQ为方差且独立于Wk和vk随机变量。wk e RnX1和vk e ΓΧ1都是均值为零的高斯白噪声， 其中￡[uA u') ] = ￡[νΑ V) ] = ￡[uA W ] = 0，δ。是克罗内克脉冲函数。
[0019] 在容积卡尔曼中，η维随机变量xk以毛为均值Pk为方差的随机变量，它可以被容 积点近似为尤…=ι;?-名+ W4其中权重值Wi为W ^
[0020] 当非线性系统信息不完全时，离散两阶容积卡尔曼滤波的分析方程如下：
[0021]
【权利要求】
1.基于非线性未知随机偏差的二阶段容积卡尔曼滤波方法，其特征在于： 系统建模，给出如下非线性系统动态模型 Xk - fk-1 (Xk-1)+Wk，k-1zk-hk (xk) +Vk 其中k彡I是时刻指数，xkeRnxi表示系统状态，Rnxi为nxI维列向量全集，ZkeRnxi 是测量值列向量，fH( ·)以及hk( ·)都是可微函数；初始状态Xtl服从元为均值，Ptl为方 差且独立于wk和Vk随机变量;WkeRnxi和VkeRmxi都是均值为零的高斯白噪声，其中 可".'，"'(]=0為可％ /] = /^;￡|>乂] = 〇，6。是克罗内克脉冲函数； 在容积卡尔曼中，η维随机变量Xk以毛为均值Pk为方差的随机变量，它可以被容积点 近似为叉 _C￡ ,c.其中权重值Wi为冰,.=丄(i=l,2,…"?;;》= 2?); 当非线性系统信息不完全时，离散两阶容积卡尔曼滤波的分析方程如下：
其中和bk为两阶容积卡尔曼的状态向量，修正无偏差估计滤波器和偏差部分分 别如下：其中下标kIk表示通过前k时刻的观测值所得到的状态估计值，同理kIk-i表示的 是根据前k-Ι个时刻的观测值得到k时刻的状态估计值； 修正无偏差部分： 步骤1.计算不含有未知偏差部分的无偏差估计一步预测，一步预测误差方差 Pk\k-lP:，k'k-.l一步预测误差协方差一1 1) .计算容积点一步递归Xi，k|H Xi，k|k-1 - f (Xi，k-l|k-i)⑴ 2) .根据m个容积点的采样来求得一步预测容积点近似估计值毛m
3) . -步预测误差估计方差，其中G,代表高斯白噪Wlrt的方差
其中右上角标X表明是不含偏差部分的方差计算，包含'代表估计值； 4) .容积点一步观测值递归 Zi，k|k-1一h (Xi,k|k-l)⑷ 5) .通过m个容积点近似一步预测观测估计值
6) .得到新息Zk以后，根据上一步得到的一步预测观测值，求其误差 Im = zk-K\k-\= zkΣ丨" （6) m 7) .观测值预测方差
8) . -步预测状态与观测协方差
步骤2.计算不含有未知偏差部分的无偏差估计增益阵--估计值元|4和估计误差方差 阵Pklk 1) .根据上两步得到一步预测增益值
其中W由MJ 2) .根据上一步的预测增益值可以得到状态估计 -Wi+[X (10) 3) .根据（3)、（7)、（10)状态估计误差方差 P叫=PUlPUf (11) 偏差估计部分： 步骤3.计算含有未知偏差部分的，未知偏差一步预测bkh偏差一步预测方差A-IA-I偏差预测误差协方差以及^#-1 1). 一步预测观测值误差
其中右上角标b表示的是含偏差部分，是由之后的耦合方程 < 求出的耦合系数； 2) . -步预测观测值误差协方差
mti' ' ................................... 3) . -步预测观测误差协方差
4) .状态偏差一步预测 bk|k-i - bk-i |k-i (16) 5) .关于误差偏差b的一步偏差预测误差方差阵A|t-i Kh+QL (17) 其中ZtllM是上一步的估计误差方差，是一步状态偏差噪声的方差值 6) .状态偏差值迭代
其中为耦合方程At= f所求的值 7) .通过（16)式算出的一步偏差预测方差阵求得/Jt 其中为稱合方程M=[乃
所求的值 步骤4.计算含有未知偏差部分的，偏差增益阵夂〗和偏差估计误差方差阵/Jt以及偏差 一步增益Ukllrf和偏差增益 1).计算关于状态误差增益系数阵
步骤5.根据以上四步计算未知偏差非线性系统的一步预测方差.和一步预测误差 协方差/i:,和最终目标状态的最优线性估计及其误差协方差7S;
将修正无偏差部分所算出来的和含偏差部分所求出来的 UklH,Klk+Uklk,bk|k，带入到离散两阶容积卡尔曼滤波的分析方程中，便可以得到 由上一步状态估计和状态估计偏差在得到信息之后得到下一步的状态估计和状态估计偏 差； 所以当知道关于偏差估计部分的初始条件：
整个滤波过程便可以从第一步开始不断估计出下一步，一直持续下去，整个滤波估计 过程得以实现。
【文档编号】H03H17/02GK104270119SQ201410487371
【公开日】2015年1月7日 申请日期:2014年9月22日 优先权日:2014年9月22日
【发明者】张露, 吕梅蕾, 王海伦, 姜春娣 申请人:衢州学院