-
摘要: 提出了一种改进的高斯近似(Gaussian approximate, GA)滤波方法, 推导了它的一般解和特殊解, 并证明了现有的高斯近似滤波方法是所提出的方法的一种特例.在提出的方法中, 不需要基于高斯假设重复地产生求积点, 而是直接地更新求积点.与现有的高斯近似滤波方法相比, 提出的方法利用了量测求积点修正状态求积点, 从而可以更好地捕获状态一步预测密度和状态后验密度的非高斯信息和高阶矩信息.此外, 提出的方法不仅适用于确定的系统模型而且还适用于随机的系统模型.单变量非平稳增长模型、垂直落体模型、再入飞行器目标跟踪的仿真验证了提出的高斯近似滤波方法的有效性和与现有方法相比的优越性.Abstract: In this paper, an improved Gaussian approximate (GA) filtering method is proposed. Its general solution and special solution are derived, and the existing GA filtering method is proved to be its special case of the proposed method. In the proposed method, the quadrature points are no longer generated repeatedly based on Gaussian assumption, but updated directly. As compared with the existing GA filtering method, the proposed method can better capture the non-Gaussian information and high-order moment information of the one-step predicted density and posterior density of state, since the measurement quadrature points in the proposed method are used to correct the state quadrature points. Moreover, the proposed method is suitable for not only deterministic process model but also random process model. The efficiency and superiority of the proposed method are illustrated by simulations of univariate non-stationary growth model, vertically falling body model, and target tracking of re-entry vehicle.
-
非线性滤波已经被广泛地应用于目标跟踪、定位、检测、信号处理、通信和自动控制中.基于随机的状态空间模型,非线性滤波的核心任务是计算状态的后验概率密度函数(Probability density function, PDF).通常可以利用贝叶斯估计理论来处理非线性滤波问题, 通过递归地计算状态的后验PDF, 贝叶斯估计理论为动态状态估计问题提供了一个最优解[1-3].但是, 对于非线性随机动态系统而言, 因为贝叶斯估计中包含的多维非线性积分一般无法解析求解, 从而无法获得状态的后验概率密度函数的解析解, 所以通常情况下不存在最优的非线性滤波器[1-3].为了完成非线性随机动态系统的状态估计任务, 必须使用近似的方法获得次优的非线性滤波器.这些近似方法可以分为两类:全局法和局部法.全局法不需要对后验PDF的形式进行明确的假设或者约束[4].粒子滤波器是一种典型的非线性全局滤波方法, 它利用随机重要性采样方法近似计算状态的后验PDF, 为非线性随机动态系统提供了渐近无偏的状态估计[5].但是它的滤波性能严重依赖于选取的重要性密度函数和使用的粒子数[6], 而且对于高维非线性状态估计应用粒子滤波具有很大的计算量[4].
局部法作为另外一种供选择的非线性近似滤波方法吸引了越来越多的研究者的注意.与全局法不一样的是局部法需要对状态的后验PDF的形式进行明确的约束.高斯近似是一种被广泛使用的有效方法, 因为在实际应用中利用该方法推导出的高斯近似滤波器具有良好的精度和计算效率[1, 7].在高斯近似滤波框架下, 状态的后验PDF被近似为高斯PDF, 它的前二阶矩为非线性随机动态系统提供了线性最小方差状态估计[1, 8].在高斯假设下, 高斯近似滤波器的核心任务是计算高斯加权积分[4].无迹卡尔曼滤波器(Unscented Kalman filter, UKF)是一种典型的高斯近似滤波器, 它利用三阶无迹变换来近似计算高斯加权积分[9-10].为了提高UKF的估计精度, Wu等基于五阶无迹变换提出了五阶UKF[11], NØrgaard等基于Stirling插值提出了分开差分滤波器(Divided difference filter, DDF)[12], Ito等基于高斯埃尔米特准则和多项式插值分别提出了高斯埃尔米特卡尔曼滤波器和中心差分卡尔曼滤波器[13].为了改善UKF在高维状态估计应用中的数值稳定性, Arasaratnam等基于三阶容积准则提出了容积卡尔曼滤波器(Cubature Kalman filter, CKF)[4].CKF因同时具有良好的数值稳定性、低的计算复杂度、容易实施等优点引起了研究者的广泛关注.但是, 三阶容积准则只具有三阶计算精度.为了提高CKF的估计精度, Jia等提出了一类具有任意阶精度的高阶CKF[14]; 基于稀疏网格理论, Jia等提出了一种稀疏网格求积滤波器[15]; 基于随机积分准则, Duník等提出了一种随机积分滤波器[16]; 基于嵌入式容积准则, Zhang等提出了一种嵌入式CKF[17-18]; 基于球面单径准则, Wang等提出了一类球面单径CKF[19]; 基于高阶无迹变换, 张勇刚等提出了一种高阶无迹卡尔曼滤波方法[20]; 基于变换的无迹变换, Chang等提出了一种变换的UKF[21]; 基于插值容积准则, Zhang等提出了一种具有任意阶精度的插值CKF[22].
以上这些高斯近似滤波器都需要假设状态一步预测PDF和状态后验PDF是高斯的, 并基于这些高斯假设重复产生求积点[23].如此的高斯假设和重复产生求积点会大大地限制高斯近似滤波器的性能, 因为它丢弃了隐藏在求积点中关于状态一步预测PDF和状态后验PDF的一些信息[23-24].比如, 当传播后的求积点具有成群的分布, 那么状态一步预测PDF和状态后验PDF不应该被约束成高斯PDF[23].此外, 基于高斯假设重复产生求积点会丢失状态一步预测PDF和状态后验PDF中的一些非高斯信息和高阶矩信息[24].为了解决这个问题, Tian等提出了一种改进的求积卡尔曼滤波器, 它不需要利用高斯假设重复产生求积点, 而是直接更新求积点[23].Cheng等将Tian等提出的方法嵌入到标准的稀疏网格求积滤波器中, 用于进一步提高稀疏网格求积滤波器的估计精度[25].
但是, 在Tian等提出的方法中, 更新的求积点仅仅是通过状态预测求积点的线性变换获得, 它忽略了量测求积点对状态求积点的修正作用.因此, Tian等提出的方法依然具有有限的估计精度.此外, Tian等提出的方法只适用于确定的系统模型, 不适用于随机的系统模型情况.为了解决这些问题, 本文首先提出了一种更新求积点的方法.在所提出的求积点更新方法中, 考虑了量测求积点对状态求积点的修正作用, 从而比现有的方法能更好地捕获状态一步预测PDF和状态后验PDF的非高斯信息和高阶矩信息.此外, 提出的求积点更新方法不仅适用于确定的系统模型而且还适用于随机的系统模型.其次, 推导了提出的求积点更新方法的一般解和特殊解, 并证明了文献[23]中的方法是本文所提出方法的一种特例.最后, 将提出的求积点更新方法嵌入到高斯近似滤波框架中, 推导出了一种新的高斯近似滤波方法.单变量非平稳增长模型、垂直落体模型、再入飞行器目标跟踪的仿真验证了提出的高斯近似滤波方法的有效性和与现有方法相比的优越性.
1. 非线性高斯近似滤波器
1.1 高斯近似滤波框架
考虑如下状态空间形式的离散非线性系统[20]
$ {\mathit{\boldsymbol{x}}_k}={\mathit{\boldsymbol{f}}_{k - 1}}({\mathit{\boldsymbol{x}}_{k - 1}})+{\mathit{\boldsymbol{w}}_{k - 1}}\qquad({\rm{系统方程}}) $
(1) $ {\mathit{\boldsymbol{z}}_k}={\mathit{\boldsymbol{h}}_k}({\mathit{\boldsymbol{x}}_k})+{\mathit{\boldsymbol{v}}_k}\qquad \qquad(量测方程) $
(2) 其中, k表示离散时间序列, xk ∈ Rn是状态向量, zk ∈ Rm是系统的量测向量, f k-1(·)和hk(·)为已知的任意函数, wk ∈ Rn和vk ∈ Rm是不相关的零均值高斯白噪声, 分别满足${\rm{E}}[{\mathit{\boldsymbol{w}}_k}\mathit{\boldsymbol{w}}_l^{\rm{T}}]={Q_k}{\delta _{kl}}$和${\rm{E}}[{\mathit{\boldsymbol{v}}_k}\mathit{\boldsymbol{v}}_l^{\rm{T}}]={R_k}{\delta _{kl}}$, 其中δkl是Kronecker-δ函数, 初始状态x0是一个均值和协方差矩阵分别为${{\mathit{\boldsymbol{\hat x}}}_{0|0}}$和P0|0的高斯随机向量, 初始状态x0, 系统噪声wk, 量测噪声vk互不相关.非线性滤波是根据当前时刻及此前的含噪声的量测来获得系统状态的最小方差估计E[xk|Z k], 其中Z k={zj, 1≤j≤k}.
在高斯近似滤波框架下, 状态和量测的联合一步预测PDF被近似为高斯, 即[4]
$ \begin{array}{l} p({\mathit{\boldsymbol{x}}_k}, {\mathit{\boldsymbol{z}}_k}|{\mathit{\boldsymbol{Z}}_{k - 1}})={\rm{N}}\left({\left[{\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{x}}_k}}\\ {{\mathit{\boldsymbol{z}}_k}} \end{array}} \right]} \right.;\left[{\begin{array}{*{20}{c}} {{{\mathit{\boldsymbol{\hat x}}}_{k|k-1}}}\\ {{{\mathit{\boldsymbol{\hat z}}}_{k|k-1}}} \end{array}} \right], \\ \qquad \quad \left.{\left[{\begin{array}{*{20}{c}} {{P_{k|k-1}}}&{{P_{\mathit{\boldsymbol{x}}z, k|k-1}}}\\ {{{({P_{\mathit{\boldsymbol{x}}z, k|k-1}})}^{\rm{T}}}}&{{P_{\mathit{\boldsymbol{z}}z, k|k - 1}}} \end{array}} \right]} \right) \end{array} $
(3) 其中, 状态的一步预测${\boldsymbol{{\hat x}}_{k|k - 1}}$和相应的协方差矩阵Pk|k-1分别为p(xk|Z k-1)的前二阶矩, 量测的一步预测${\boldsymbol{{\hat z}}_{k|k - 1}}$和相应的协方差矩阵Pzz, k|k-1分别为p(zk|Z k-1)的前二阶矩, Pxz, k|k-1为状态和量测的互协方差矩阵, 并且它们可以被计算如下[4]:
$ \begin{array}{l} {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}=\int_{{{\bf{R}}^n}} {{\mathit{\boldsymbol{f}}_{k - 1}}}({\mathit{\boldsymbol{x}}_{k - 1}}){\rm{N}}({\mathit{\boldsymbol{x}}_{k - 1}};{{\mathit{\boldsymbol{\hat x}}}_{k - 1|k - 1}}, \\ \qquad {P_{k - 1|k - 1}}){\rm{d}}{\mathit{\boldsymbol{x}}_{k - 1}} \end{array} $
(4) $ \begin{array}{l} {P_{k|k - 1}}=\int_{{{\bf{R}}^n}} {{\mathit{\boldsymbol{f}}_{k - 1}}}({\mathit{\boldsymbol{x}}_{k - 1}})\mathit{\boldsymbol{f}}_{k - 1}^{\rm{T}}({\mathit{\boldsymbol{x}}_{k - 1}}){\rm{N}}({\mathit{\boldsymbol{x}}_{k - 1}};{\rm{ }}\\ \qquad {{\mathit{\boldsymbol{\hat x}}}_{k - 1|k - 1}}, {P_{k - 1|k - 1}}){\rm{d}}{\mathit{\boldsymbol{x}}_{k - 1}} - {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}\mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\rm{T}}+{\rm{ }}\\ \qquad {Q_{k - 1}} \end{array} $
(5) $ {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}=\int_{{{\bf{R}}^n}} {{\mathit{\boldsymbol{h}}_k}}({\mathit{\boldsymbol{x}}_k}){\rm{N}}({\mathit{\boldsymbol{x}}_k};{{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}, {P_{k|k - 1}}){\rm{d}}{\mathit{\boldsymbol{x}}_k} $
(6) $ \begin{array}{l} {P_{\mathit{\boldsymbol{z}}z, k|k - 1}}=\int_{{{\bf{R}}^n}} {{\mathit{\boldsymbol{h}}_k}}({\mathit{\boldsymbol{x}}_k})\mathit{\boldsymbol{h}}_k^{\rm{T}}({\mathit{\boldsymbol{x}}_k}){\rm{N}}({\mathit{\boldsymbol{x}}_k};{{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}, \\ \qquad {P_{k|k - 1}}){\rm{d}}{\mathit{\boldsymbol{x}}_k} - {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}\mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\rm{T}}+{R_k} \end{array} $
(7) $ \begin{array}{l} {P_{\mathit{\boldsymbol{x}}z, k|k - 1}}=\int_{{{\bf{R}}^n}} {{\mathit{\boldsymbol{x}}_k}} \mathit{\boldsymbol{h}}_k^{\rm{T}}({\mathit{\boldsymbol{x}}_k}){\rm{N}}({\mathit{\boldsymbol{x}}_k};{{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}, {P_{k|k - 1}})\times \\ \qquad {\rm{d}}{\mathit{\boldsymbol{x}}_k} - {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}\mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\rm{T}} \end{array} $
(8) 根据式(3)并利用贝叶斯准则, 状态的后验PDF可以被更新为高斯[1]:
$ p({\mathit{\boldsymbol{x}}_k}|{\mathit{\boldsymbol{Z}}_k})=\frac{{p({\mathit{\boldsymbol{x}}_k}, {\mathit{\boldsymbol{z}}_k}|{\mathit{\boldsymbol{Z}}_{k - 1}})}}{{p({\mathit{\boldsymbol{z}}_k}|{\mathit{\boldsymbol{Z}}_{k - 1}})}}={\rm{N}}({\mathit{\boldsymbol{x}}_k};{{\mathit{\boldsymbol{\hat x}}}_{k|k}}, {P_{k|k}}) $
(9) 其中, 状态估计${\boldsymbol{{\hat x}}_{k|k}}$和相应的估计误差协方差矩阵Pk|k 可以被计算如下:
$ {{\mathit{\boldsymbol{\hat x}}}_{k|k}}={{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}+{W_k}({\mathit{\boldsymbol{z}}_k} - {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}) $
(10) $ {P_{k|k}}={P_{k|k - 1}} - {W_k}{P_{zz, k|k - 1}}W_k^{\rm{T}} $
(11) $ {W_k}={P_{\mathit{\boldsymbol{x}}z, k|k - 1}}P_{\mathit{\boldsymbol{z}}z, k|k - 1}^{ - 1} $
(12) 其中, Wk为卡尔曼滤波增益.
高斯近似滤波器由时间更新和量测更新组成, 其中时间更新包括方程(4)和(5), 量测更新包括方程(6)~(8)和方程(10)~(12).高斯近似滤波器的核心任务是如何计算方程(4)~(8)中的高斯加权积分.这些高斯加权积分可以写成如下统一的形式:
$ I[\mathit{\boldsymbol{g}}]=\int_{{{\bf{R}}^n}} \mathit{\boldsymbol{g}}(\mathit{\boldsymbol{x}}){\rm{N}}(\mathit{\boldsymbol{x}};\mathit{\boldsymbol{\mu }}, \Sigma){\rm{d}}\mathit{\boldsymbol{x}} $
(13) 在一般的情况下, 我们不可能解析地求解方程(13)中的积分, 需要采用如下的求积准则来近似计算它:
$ \int_{{{\bf{R}}^n}} \mathit{\boldsymbol{g}}(\mathit{\boldsymbol{x}}){\rm{N}}(\mathit{\boldsymbol{x}};\mathit{\boldsymbol{\mu }}, \Sigma){\rm{d}}\mathit{\boldsymbol{x}} \approx \sum\limits_{i=1}^N {{\omega _i}} \mathit{\boldsymbol{g}}({\mathit{\boldsymbol{x}}_i}) $
(14) 其中, xi和ωi分别为高斯密度N(x; μ, Σ)的求积点和相应的权值, N为求积点的数量.利用方程(14)中的求积准则来计算方程(4)~(8)中高斯加权积分, 不同的选取求积点xi和计算权值ωi的方法导致了不同的高斯近似滤波器, 比如基于无迹变换的UKF[9-10]、基于三阶容积准则的CKF[4]、基于嵌入式容积准则的嵌入式CKF[18]、基于高阶无迹变换的高阶UKF[20]、基于插值容积准则的插值CKF[22].
现有的高斯近似滤波方法在时间更新和量测更新之前需要基于高斯假设产生求积点, 在时间更新和量测更新之后这些求积点都将被丢弃, 然而在下一次时间更新和量测更新时, 又需要基于高斯假设重新产生这些求积点.基于高斯假设重新产生的求积点只能分别保留状态后验PDF p(xk|Z k)和状态一步预测PDF p(xk|Z k-1)的低阶矩信息(比如三阶容积准则只能保留前三阶矩, 五阶容积准则只能保留前五阶矩), 高阶矩信息将被丢失掉, 从而限制了现有高斯近似滤波方法的估计精度[23].此外, 在丢弃求积点和基于高斯假设重新产生求积点的过程中, 部分非高斯信息也将被丢失掉[23].为了解决这个问题, Tian等提出了一种改进的求积卡尔曼滤波器[23].
1.2 现有的改进求积卡尔曼滤波器
文献[23]中的改进求积卡尔曼滤波器不需要利用高斯假设重复产生求积点, 而是直接更新求积点.在给出文献[23]中的改进求积卡尔曼滤波方法之前, 我们定义如下的变量:
$ \mathit{\boldsymbol{\tilde \xi }}_k^ -=[\mathit{\boldsymbol{\xi }}_k^{(1)-}-{{\mathit{\boldsymbol{\hat x}}}_{k|k-1}} \cdots \mathit{\boldsymbol{\xi }}_k^{(N)- }- {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}] $
(15) $ \mathit{\boldsymbol{\tilde \xi }}_k^+=[\mathit{\boldsymbol{\xi }}_k^{(1)+}-{{\mathit{\boldsymbol{\hat x}}}_{k|k}} \cdots \mathit{\boldsymbol{\xi }}_k^{(N)+}-{{\mathit{\boldsymbol{\hat x}}}_{k|k}}] $
(16) 其中, $\boldsymbol{\xi} _k^{(i)- }$为一步预测状态的求积点, $\boldsymbol{\tilde \xi} _k^ - $为状态预测求积点误差矩阵, $\boldsymbol{\xi} _k^{(i)+}$为k时刻更新的求积点, $\boldsymbol{\tilde \xi} _k^+$为状态更新求积点误差矩阵.基于方程(14)中的求积准则和方程(15)和(16)中的定义, 文献[23]中的改进求积卡尔曼滤波方法可被统一描述如下[23]:
1)初始化:根据方程(14)中的方法和相应的求积准则(比如无迹变换[9])产生密度函数$({\mathit{\boldsymbol{x}}_0};{{\mathit{\boldsymbol{\hat x}}}_{0|0}}, {P_{0|0}})$的求积点$\boldsymbol{\xi} _0^{(i)+}$和相应的权值ωi.
2)时间更新
a)传播上一时刻更新的求积点$\boldsymbol{\xi} _{k - 1}^{(i)+}$.
$ \mathit{\boldsymbol{\xi }}_k^{(i)- }={\mathit{\boldsymbol{f}}_{k - 1}}(\mathit{\boldsymbol{\xi }}_{k - 1}^{(i)+}) $
(17) b)计算状态的一步预测${\boldsymbol{{\hat x}}_{k|k - 1}}$和相应的误差协方差矩阵Pk|k-1.
$ {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}=\sum\limits_{i=1}^N {{\omega _i}} \mathit{\boldsymbol{\xi }}_k^{(i)- } $
(18) $ {P_{k|k - 1}}=\sum\limits_{i=1}^N {{\omega _i}} \mathit{\boldsymbol{\xi }}_k^{(i)- }{(\mathit{\boldsymbol{\xi }}_k^{(i)- })^{\rm{T}}} - {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}\mathit{\boldsymbol{\hat x}}_{k|k - 1}^{\rm{T}} $
(19) c)利用方程(15)计算状态预测求积点误差矩阵$\boldsymbol{\tilde \xi} _k^ - $.
3)量测更新
a)计算量测的一步预测${\boldsymbol{{\hat z}}_{k|k - 1}}$和相应的协方差矩阵Pzz, k|k-1, 状态和量测的互协方差矩阵Pxz, k|k-1.
$ {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}=\sum\limits_{i=1}^N {{\omega _i}} {\mathit{\boldsymbol{h}}_k}(\mathit{\boldsymbol{\xi }}_k^{(i)- }) $
(20) $ \begin{array}{l} {P_{\mathit{\boldsymbol{z}}z, k|k - 1}}=\sum\limits_{i=1}^N {{\omega _i}} {\mathit{\boldsymbol{h}}_k}(\mathit{\boldsymbol{\xi }}_k^{(i)- })\mathit{\boldsymbol{h}}_k^{\rm{T}}(\mathit{\boldsymbol{\xi }}_k^{(i)- })- {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}} \times \\ \qquad \mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\rm{T}}+{R_k} \end{array} $
(21) $ {P_{\mathit{\boldsymbol{x}}z, k|k - 1}}=\sum\limits_{i=1}^N {{\omega _i}} \mathit{\boldsymbol{\xi }}_k^{(i)- }\mathit{\boldsymbol{h}}_k^{\rm{T}}(\mathit{\boldsymbol{\xi }}_k^{(i)- })- {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}\mathit{\boldsymbol{\hat z}}_{k|k - 1}^{\rm{T}} $
(22) b)利用方程(12)计算卡尔曼滤波增益Wk, 并利用方程(10)和(11)更新状态估计${\boldsymbol{{\hat x}}_{k|k}}$和相应的估计误差协方差矩阵Pk|k .
c)计算状态更新求积点误差矩阵$\boldsymbol{\tilde \xi} _k^+$
$ \mathit{\boldsymbol{\tilde \xi }}_k^+={S_{k|k}}S_{k|k - 1}^{ - 1}\mathit{\boldsymbol{\tilde \xi }}_k^ - $
(23) 其中, Sk|k和Sk|k-1分别为Pk|k和Pk|k-1的方根矩阵.
d)计算更新的求积点$\boldsymbol{\xi} _k^{(i)+}$
$ [\mathit{\boldsymbol{\xi }}_k^{(1)+} \cdots \mathit{\boldsymbol{\xi }}_k^{(N)+}]=\mathit{\boldsymbol{\tilde \xi }}_k^++[{{\mathit{\boldsymbol{\hat x}}}_{k|k}} \cdots {{\mathit{\boldsymbol{\hat x}}}_{k|k}}] $
(24) 从方程(23)中可以看出, 状态更新求积点误差矩阵$\boldsymbol{\tilde \xi} _k^+$仅是通过状态预测求积点误差矩阵$\boldsymbol{\tilde \xi} _k^ -$的线性变换获得, 而与量测预测求积点误差矩阵无关, 它忽略了量测求积点对状态求积点的修正作用, 从而限制了文献[23]中提出的改进求积卡尔曼滤波的估计精度.此外, 文献[23]只针对带确定系统模型的非线性系统(即式(3)中的系统噪声wk-1=0)设计改进的求积卡尔曼滤波器, 因此它不适用于随机的系统模型(即式(3)中的系统噪声wk-1 ≠ 0), 因此在式(19)中并无系统噪声方差矩阵项.为了解决文献[23]中的问题, 本文将提出一种改进的高斯近似滤波方法.提出的方法利用了量测求积点修正状态求积点, 从而比文献[23]中的方法具有更高的估计精度.此外, 提出的方法不仅适用于确定的系统模型而且还适用于随机的系统模型.
2. 一种改进的高斯近似滤波方法
2.1 求积点的更新
在提出新的求积点更新方法之前, 我们定义如下的变量.
$ \mathit{\boldsymbol{\chi }}_k^{(i)}={\mathit{\boldsymbol{f}}_{k - 1}}(\mathit{\boldsymbol{X}}_{k - 1}^{(i)+}) $
(25) $ {{\mathit{\boldsymbol{\tilde \chi }}}_k}=[\mathit{\boldsymbol{\chi }}_k^{(1)}-{{\mathit{\boldsymbol{\hat x}}}_{k|k-1}} \cdots \mathit{\boldsymbol{\chi }}_k^{(N)}-{{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}] $
(26) $ \mathit{\boldsymbol{\tilde X}}_k^ -=[\mathit{\boldsymbol{X}}_k^{(1)-}-{{\mathit{\boldsymbol{\hat x}}}_{k|k-1}} \cdots \mathit{\boldsymbol{X}}_k^{(N)- }- {{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}] $
(27) $ \mathit{\boldsymbol{Z}}_k^{(i)- }={\mathit{\boldsymbol{h}}_k}(\mathit{\boldsymbol{X}}_k^{(i)- }) $
(28) $ \mathit{\boldsymbol{\tilde Z}}_k^ -=[\mathit{\boldsymbol{Z}}_k^{(1)-}-{{\mathit{\boldsymbol{\hat z}}}_{k|k-1}} \cdots \mathit{\boldsymbol{Z}}_k^{(N)- }- {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}] $
(29) $ \begin{align} \tilde{\boldsymbol {X}}_{k}^{+}=[{\boldsymbol X}_{k}^{(1)+}-\hat{\boldsymbol {x}}_{k|k}\cdots{\boldsymbol X}_{k}^{(N)+}-\hat{\boldsymbol {x}}_{k|k}] \end{align} $
(30) $ \begin{align} {\boldsymbol \omega}=[\omega_{1}\cdots\omega_{N}]^{{\rm T}}, \qquad W=\mathrm{diag}\{{\boldsymbol \omega}\} \end{align} $
(31) 其中, $\boldsymbol{X}_{k - 1}^{(i)+}$为k-1时刻更新的求积点, $\boldsymbol{{ \chi}}_{k}^{(i)}$为$\boldsymbol{X}_{k - 1}^{(i)+}$经系统函数f k-1(·)传播的求积点, ${\boldsymbol{{\tilde \chi }}_k}$为传播求积点的误差矩阵, $\boldsymbol{X}_k^{(i)- }$为一步预测状态的求积点, $\boldsymbol{\tilde X}_k^ - $状态预测求积点误差矩阵, Z k(i)-为一步预测量测的求积点, $\boldsymbol{\tilde Z}_k^ - $为量测预测求积点误差矩阵, X k(i)+为k时刻更新的求积点, $\boldsymbol{\tilde X}_k^+$为状态更新求积点误差矩阵, ω为一个由权值组成的列向量, W 为一个由权值构成对角线的对角矩阵.根据${{\mathit{\boldsymbol{\hat x}}}_{k|k - 1}}, {P_{k|k - 1}}, {{\mathit{\boldsymbol{\hat z}}}_{k|k - 1}}, {P_{\mathit{\boldsymbol{z}}z, k|k - 1}}, {P_{\mathit{\boldsymbol{x}}z, k|k - 1}}$的定义, 我们可以得到如下的关系式:
$ \begin{align} \tilde{\boldsymbol {\chi}}_{k}{\boldsymbol \omega}={\boldsymbol 0} \end{align} $
(32) $ \begin{align} \tilde{\boldsymbol {\chi}}_{k}W\tilde{\boldsymbol {\chi}}_{k}^{{\rm T}}={P}_{k|k-1}-{Q}_{k-1} \end{align} $
(33) $ \begin{align} \tilde{\boldsymbol {Z}}_{k}^{-}{\boldsymbol \omega}={\boldsymbol 0} \end{align} $
(34) $ \begin{align} \tilde{\boldsymbol {Z}}_{k}^{-}W(\tilde{\boldsymbol {Z}}_{k}^{-})^{{\rm T}}={P}_{{\boldsymbol zz}, k|k-1}-{R}_{k} \end{align} $
(35) $ \begin{align} \tilde{{X}}_{k}^{-}W(\tilde{{Z}}_{k}^{-})^{{\rm T}}={P}_{{xz}, k|k-1} \end{align} $
(36) 在本节中, 我们将采用如下的方法更新求积点:
$ \begin{align} \tilde{{X}}_{k}^{-}=F\tilde{{\chi}}_{k} \end{align} $
(37) $ \begin{align} \tilde{{X}}_{k}^{+}=G\tilde{{X}}_{k}^{-}-H\tilde { {Z}}_{k}^{-} \end{align} $
(38) 满足如下约束
$ \begin{align} \tilde{{X}}_{k}^{-}{\omega}={0} \end{align} $
(39) $ \begin{align} \tilde{\boldsymbol {X}}_{k}^{-}W(\tilde{\boldsymbol {X}}_{k}^{-})^{{\rm T}}={P}_{k|k-1} \end{align} $
(40) $ \begin{align} \tilde{\boldsymbol {X}}_{k}^{+}{\boldsymbol \omega}={\boldsymbol 0} \end{align} $
(41) $ \begin{align} \tilde{\boldsymbol {X}}_{k}^{+}W(\tilde{\boldsymbol {X}}_{k}^{+})^{{\rm T}}={P}_{k|k} \end{align} $
(42) 其中, Pk|k在方程(11)中给出.方程(39)~(42)中的约束表示更新后的求积点X k(i)+和X k(i)-至少能分别匹配状态后验密度p(xk|Z k)和状态一步预测密度p(xk|Z k-1)的均值和协方差矩阵, 同时保持求积点的权值ωi不变.当我们获得$\boldsymbol{\tilde \xi} _k^ -$和$\boldsymbol{\tilde \xi} _k^+$后, 状态预测的求积点X k(i)-和状态更新的求积点X k(i)+可以分别通过在$\boldsymbol{\tilde \xi} _k^ -$和$\boldsymbol{X}_{k - 1}^{(i)+}$的每一列上分别加上状态一步预测${\boldsymbol{{\hat x}}_{k|k - 1}}$和状态估计${\boldsymbol{{\hat x}}_{k|k}}$得到.
注1. 文献[23]只是针对带确定系统模型的非线性系统(即系统噪声协方差矩阵Qk-1=0)设计求积点更新方法, 从而它只适用于确定的系统模型.然而, 从方程(33)中可以看到, 本文是直接针对一般的带随机系统模型的非线性系统设计求积点更新方法, 从而提出的方法不仅适用于确定的系统模型, 而且适用于随机的系统模型.从方程(23)中可以看到, 文献[23]忽略了量测求积点对状态求积点的修正作用.然而从方程(38)中可以看到, 提出的求积点更新方法利用了量测求积点对状态求积点进行修正, 因此比文献[23]中的方法能更好地更新状态求积点.
接下来, 我们将在定理1中给出矩阵F的计算方法, 在定理2和定理3中给出矩阵G和H的计算方法.
定理1. 如果矩阵F满足如下关系式:
$ F=S_{k|k-1}M_{1}^{{\rm T}}(S_{k|k-1}^{1})^{-1} $
(43) 其中, Sk|k-1为Pk|k-1的方根矩阵, M1为任意的正交矩阵, Sk|k-11为Pk|k-1-Qk-1的方根矩阵, 即
$ S_{k|k-1}S_{k|k-1}^{{\rm T}}=P_{k|k-1}, \qquad M_{1}M_{1}^{{\rm T}}=I $
(44) $ S_{k|k-1}^{1}(S_{k|k-1}^{1})^{{\rm T}}={P}_{k|k-1}-{Q}_{k-1} $
(45) 其中, I为单位矩阵.那么约束方程(39)和(40)成立.
证明. 利用方程(32)和(37), 可以得到:
$ \begin{align} \tilde{\boldsymbol {X}}_{k}^{-}{\boldsymbol \omega}=F\tilde {\boldsymbol {\chi}}_{k}{\boldsymbol \omega}=F{\boldsymbol 0}={\boldsymbol 0} \end{align} $
(46) 从而约束方程(39)对于任意的矩阵F都成立.
根据方程(33)和方程(37), 可以计算矩阵$\tilde {\boldsymbol {X}}_{k}^{-}W(\tilde{\boldsymbol {X}}_{k}^{-})^{{\rm T}}$如下:
$ \begin{align} &\tilde{\boldsymbol {X}}_{k}^{-}W(\tilde{\boldsymbol {X}}_{k}^{-})^{{\rm T}}=F\tilde{\boldsymbol {\chi}}_{k}W(F\tilde{\boldsymbol {\chi}}_{k})^{{\rm T}}=F\tilde{\boldsymbol {\chi}}_{k}W\tilde{\boldsymbol {\chi}}_{k}^{{\rm T}}F^{{\rm T}}=\nonumber\\ &\qquad F({P}_{k|k-1}-{Q}_{k-1})F^{{\rm T}} \end{align} $
(47) 通过将方程(47)代入到方程(40)中, 可以得到与约束方程(40)等价的约束方程:
$ F({P}_{k|k-1}-{Q}_{k-1})F^{{\rm T}}=P_{k|k-1} $
(48) 根据方程(43)~(45), $F({P}_{k|k-1}-{Q}_{k-1})F^{{\rm T}}$可以被计算如下:
$ \begin{align} &F({P}_{k|k-1}-{Q}_{k-1})F^{{\rm T}}=S_{k|k-1}M_{1}^{{\rm T}}(S_{k|k-1}^{1})^{-1}\times \nonumber\\ &\quad({P}_{k|k-1}-{Q}_{k-1})(S_{k|k-1}M_{1}^{{\rm T}}(S_{k|k-1}^{1})^{-1})^{{\rm T}}=\nonumber\\ &\quad S_{k|k-1}M_{1}^{{\rm T}}(S_{k|k-1}^{1})^{-1}S_{k|k-1}^{1}(S_{k|k-1}^{1})^{{\rm T}}\times \nonumber\\ &\quad((S_{k|k-1}^{1})^{{\rm T}})^{-1} M_{1}S_{k|k-1}^{{\rm T}}=S_{k|k-1}S_{k|k-1}^{{\rm T}}=P_{k|k-1} \end{align} $
(49) 通过方程(49)可以知道, 如果F满足方程(43)~(45), 那么约束方程(48)成立, 从而与之等价的约束方程(40)也成立.综上所述, 如果矩阵F满足方程(43)~(45), 那么约束方程(39)和(40)成立.
接下来我们将计算矩阵G和H.为了方便计算, 我们将矩阵G和H的求解分成如下两步:首先找到满足如下约束方程的误差矩阵$\tilde {\boldsymbol {X}}_{k}^{'+}$.
$ \mathit{\boldsymbol{\tilde X}}_k^{'+}=A\mathit{\boldsymbol{\tilde X}}_k^ - - B\mathit{\boldsymbol{\tilde z}}_k^ - $
(50) $ \mathit{\boldsymbol{\tilde X}}_k^{'+}\mathit{\boldsymbol{\omega }}={\bf{0}} $
(51) $ \mathit{\boldsymbol{\tilde X}}_k^{'+}+W{\left({\mathit{\boldsymbol{\tilde X}}_k^{'+}} \right)^{\rm{T}}}={P_{k|k}} - {W_k}{R_k}W_k^{\rm{T}} $
(52) 然后通过如下的线性变换将$\boldsymbol{\tilde X}_k^{'+}$转化成所需要的$\boldsymbol{\tilde X}_k^+$.
$ \begin{align} \tilde{\boldsymbol {X}}_{k}^{+}=L\tilde{\boldsymbol {X}}_{k}^{'+} \end{align} $
(53) 利用方程(38), (50)和(53), 可以得到:
$ \begin{align} \tilde{\boldsymbol {X}}_{k}^{+}=LA\tilde{\boldsymbol {X}}_{k}^{-}-LB\tilde {\boldsymbol {Z}}_{k}^{-}=G\tilde{\boldsymbol {X}}_{k}^{-}-H\tilde {\boldsymbol {Z}}_{k}^{-} \end{align} $
(54) 从而所需的矩阵G和H可以被计算如下:
$ G=LA, \qquad H=LB $
(55) 定理2. 如果矩阵A和B满足如下关系式:
$ \begin{align} A=B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+EM_{2}^{{\rm T}}S_{k|k-1}^{-1} \end{align} $
(56) $ \begin{array}{l} E{E^{\rm{T}}}={P_{k|k}} - {W_k}{R_k}W_k^{\rm{T}} - B[{P_{zz, k|k-1}}-{R_k}-\qquad \\ \;\;\;\;\;\;\;\;\;\;\;P_{\mathit{\boldsymbol{x}}z, k|k - 1}^{\rm{T}}P_{k|k - 1}^{ - 1}{P_{\mathit{\boldsymbol{x}}z, k|k - 1}}]{B^{\rm{T}}} \end{array} $
(57) 其中, Wk 为卡尔曼滤波增益, M2为任意的正交矩阵, 即
$ M_{2}M_{2}^{{\rm T}}=I $
(58) 那么约束方程(51)和(52)成立.
证明. 利用方程(34), (39), (50), 可以得到:
$ \begin{align} &\tilde{\boldsymbol {X}}_{k}^{'+}{\boldsymbol \omega}=(A\tilde{\boldsymbol {X}}_{k}^{-}-B\tilde{\boldsymbol {Z}}_{k}^{-}){\boldsymbol \omega}=A\tilde{\boldsymbol {X}}_{k}^{-}{\boldsymbol \omega}-B\tilde{\boldsymbol {Z}}_{k}^{-}{\boldsymbol \omega}=\nonumber\\ &\qquad A{\boldsymbol 0}-B{\boldsymbol 0}={\boldsymbol 0} \end{align} $
(59) 从而约束方程(51)对于任意的矩阵A和B都成立.
利用方程(50), $\tilde{\boldsymbol {X}}_{k}^{'+}W(\tilde {\boldsymbol {X}}_{k}^{'+})^{{\rm T}}$可以被计算如下:
$ \begin{align} &\tilde{\boldsymbol {X}}_{k}^{'+}W(\tilde{\boldsymbol {X}}_{k}^{'+})^{{\rm T}}=\notag\\ &\qquad(A\tilde{\boldsymbol {X}}_{k}^{-}-B\tilde{\boldsymbol {Z}}_{k}^{-})W(A\tilde{\boldsymbol {X}}_{k}^{-}-B\tilde{\boldsymbol {Z}}_{k}^{-})^{{\rm T}}=\nonumber\\ &\qquad A(\tilde{\boldsymbol {X}}_{k}^{-}W(\tilde{\boldsymbol {X}}_{k}^{-})^{{\rm T}})A^{{\rm T}}-B(\tilde{\boldsymbol {X}}_{k}^{-}W(\tilde{\boldsymbol {Z}}_{k}^{-})^{{\rm T}})^{{\rm T}}A^{{\rm T}}- \nonumber\\ &\qquad A(\tilde{\boldsymbol {X}}_{k}^{-}W(\tilde{\boldsymbol {Z}}_{k}^{-})B^{{\rm T}}+B(\tilde{\boldsymbol {Z}}_{k}^{-}W(\tilde{\boldsymbol {Z}}_{k}^{-})^{{\rm T}})B^{{\rm T}} \end{align} $
(60) 将方程(35), (36), (40)代入到方程(60)中, 可以得到:
$ \begin{align} &\tilde{\boldsymbol {X}}_{k}^{'+}W(\tilde{\boldsymbol {X}}_{k}^{'+})^{{\rm T}}=AP_{k|k-1}A^{{\rm T}}-B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}A^{{\rm T}}- \nonumber\\ &\qquad A{P}_{{\boldsymbol xz}, k|k-1}B^{{\rm T}}+B({P}_{{\boldsymbol zz}, k|k-1}-R_{k})B^{{\rm T}} \end{align} $
(61) 通过将方程(61)代入到方程(52)中, 我们可以得到一个与约束方程(52)等价的约束方程:
$ \begin{align} &AP_{k|k-1}A^{{\rm T}}-B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}A^{{\rm T}}-A{P}_{{\boldsymbol xz}, k|k-1}B^{{\rm T}}+\nonumber\\ &\qquad B({P}_{{\boldsymbol zz}, k|k-1}-R_{k})B^{{\rm T}}={P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}} \end{align} $
(62) 利用矩阵分解, 我们可以得到一个与约束方程(62)等价的约束方程:
$ \begin{align} &AS_{k|k-1}(AS_{k|k-1})^{{\rm T}}-B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}\times \nonumber\\ &\quad(AS_{k|k-1})^{{\rm T}}-(AS_{k|k-1})S_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}B^{{\rm T}}+\nonumber\\ &\quad B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}B^{{\rm T}}={P}_{k|k}- \nonumber\\ &\quad {W}_{k}R_{k}{W}_{k}^{\rm{T}}-B[{P}_{{\boldsymbol zz}, k|k-1}-R_{k}-\nonumber\\ &\quad {P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}]B^{{\rm T}} \end{align} $
(63) 通过矩阵运算, 约束方程(63)能够被等价地表示如下:
$ \begin{align} &\{[AS_{k|k-1}-B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}]\}\{[AS_{k|k-1}-\nonumber\\ &\quad B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}]\}^{{\rm T}}={P}_{k|k}- \nonumber\\ &\quad {W}_{k}R_{k}{W}_{k}^{\rm{T}}-B[{P}_{{\boldsymbol zz}, k|k-1}-R_{k}-\nonumber\\ &\quad {P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}]B^{{\rm T}} \end{align} $
(64) 根据方程(56), $AS_{k|k-1}-B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}$可以被计算如下:
$ \begin{align} &AS_{k|k-1}-B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}=\notag\\ &\quad(B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+\nonumber\\ &\quad EM_{2}^{{\rm T}}S_{k|k-1}^{-1})S_{k|k-1}-B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}=\nonumber\\ &\quad B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}(S_{k|k-1}^{{\rm T}})^{-1}+EM_{2}^{{\rm T}}- \nonumber\\ &\quad B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}(S_{k|k-1}^{{\rm T}})^{-1}=EM_{2}^{{\rm T}} \end{align} $
(65) 利用方程(57)和(58)和方程(65), 矩阵$\{[A\times$S_{k|k-1}-B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}]\}\{[AS_{k|k-1}-B(S_{k|k-1}^{-1}$ $\times P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}]\}^{{\rm T}}$可以被计算如下:
$ \begin{align} &\{[AS_{k|k-1}-B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}]\}\{[AS_{k|k-1}-\nonumber\\ &\quad B(S_{k|k-1}^{-1}P_{{\boldsymbol xz}, k|k-1})^{{\rm T}}]\}^{{\rm T}}=EM_{2}^{{\rm T}}M_{2}E^{{\rm T}}=\nonumber\\ &\quad {P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}}-B[{P}_{{\boldsymbol zz}, k|k-1}-R_{k}-\nonumber\\ &\quad {P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}]B^{{\rm T}} \end{align} $
(66) 通过方程(66)可以知道, 如果矩阵 A和B满足方程(56)~(58), 那么约束方程(62)和(63)成立, 从而与之等价的约束方程(52)也成立.综上所述, 如果矩阵 A和B满足方程(56)~(58), 那么约束方程(51)和(52)成立.
定理3. 如果矩阵L满足如下关系式:
$ L=S_{k|k}M_{3}^{{\rm T}}D^{-1} $
(67) 其中, Sk|k为Pk|k的方根矩阵, M3为任意的正交矩阵, D为Pk|k -WkRkWk T的方根矩阵, 即
$ S_{k|k}S_{k|k}^{\rm T}=P_{k|k}, \quad M_{3}M_{3}^{{\rm T}}=I $
(68) $ DD^{{\rm T}}={P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}} $
(69) 那么约束方程(41)和(42)成立.
证明. 利用方程(51)和(53), $\tilde{\boldsymbol {x}}_{k}^{+}{\boldsymbol \omega}$可以被计算如下:
$ \begin{align} \tilde{\boldsymbol {X}}_{k}^{+}{\boldsymbol \omega}=L\tilde {\boldsymbol {X}}_{k}^{'+}{\boldsymbol \omega}=L{\boldsymbol 0}={\boldsymbol 0} \end{align} $
(70) 从而约束方程(41)对于任意的矩阵L都成立.
利用方程(52)和(53), $\tilde{\boldsymbol {X}}_{k}^{+}W(\tilde {\boldsymbol {X}}_{k}^{+})^{{\rm T}}$可以被计算为
$ \begin{align} &\tilde{\boldsymbol {X}}_{k}^{+}W(\tilde{\boldsymbol {X}}_{k}^{+})^{{\rm T}}=L\tilde{\boldsymbol {X}}_{k}^{'+}W(L\tilde{\boldsymbol {X}}_{k}^{'+})^{{\rm T}}=\nonumber\\ &\qquad L\tilde{\boldsymbol {X}}_{k}^{'+}W(\tilde{\boldsymbol {X}}_{k}^{'+})^{{\rm T}}L^{{\rm T}}=L({P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}})L^{{\rm T}} \end{align} $
(71) 通过将方程(71)代入到方程(42)中, 我们可以得到与约束方程(42)等价的约束方程:
$ L({P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}})L^{{\rm T}}={P}_{k|k} $
(72) 根据方程(67)~(69), $L({P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}})L^{{\rm T}}$可以被计算如下:
$ \begin{align} &L({P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}})L^{{\rm T}}=S_{k|k}M_{3}^{{\rm T}}D^{-1}({P}_{k|k}- \nonumber\\ &\qquad {W}_{k}R_{k}{W}_{k}^{\rm{T}})(S_{k|k}M_{3}^{{\rm T}}D^{-1})^{{\rm T}}=\nonumber\\ &\qquad S_{k|k}M_{3}^{{\rm T}}D^{-1}DD^{{\rm T}}(D^{{\rm T}})^{-1}M_{3}S_{k|k}^{{\rm T}}=\nonumber\\ &\qquad S_{k|k}S_{k|k}^{\rm T}=P_{k|k} \end{align} $
(73) 通过方程(73)可以知道, 如果矩阵L满足方程(67)~(69), 那么约束方程(72)成立, 从而与之等价的约束方程(42)也成立.综上所述, 如果矩阵L满足方程(67)~(69), 那么约束方程(41)和(42)成立.
综合定理2和定理3中的结果, 所需矩阵G和H的一般表达式可以表示为
$ G=S_{k|k}M_{3}^{{\rm T}}D^{-1}(B{P}_{{\boldsymbol{x}z}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+EM_{2}^{{\rm T}}S_{k|k-1}^{-1}) $
(74) $ H=S_{k|k}M_{3}^{{\rm T}}D^{-1}B $
(75) 从方程(74)和(75)中可以看出, 矩阵G和H都是未知矩阵$B$的函数.为了获得矩阵G和H具体计算表达式, 我们需要进一步给出矩阵B的具体值.为此, 接下来我们将给出矩阵B两种取值可能, 推导出本文提出方法的两种特解.首先, 我们考虑矩阵B=0的情况.在矩阵B=0时, 状态更新求积点误差矩阵$\tilde {\boldsymbol {X}}_{k}^{+}$只与状态预测求积点误差矩阵$\tilde {\boldsymbol {X}}_{k}^{-}$相关, 而与量测预测求积点误差矩阵$\tilde {\boldsymbol {Z}}_{k}^{-}$无关.在这种情况下, 矩阵G和H的解将在如下的定理4中给出.
定理4. 如果矩阵B=0, 并且如下矩阵不等式成立:
$ {P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}}>0 $
(76) 那么矩阵G和H可以被计算为
$ G=S_{k|k}M_{4}^{{\rm T}}S_{k|k-1}^{-1}, \qquad H=0 $
(77) 其中, M4为任意的正交矩阵.
$ M_{4}M_{4}^{{\rm T}}=I $
(78) 证明. 将B=0代入到方程(75)中, 可以得到:
$ H=S_{k|k}M_{3}^{{\rm T}}D^{-1}B=S_{k|k}M_{3}^{{\rm T}}D^{-1}0=0 $
(79) 将B=0代入方程(57)中, 可以得到:
$ EE^{{\rm T}}=h{P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}} $
(80) 利用方程(69)和(80), 可以得到:
$ DD^{{\rm T}}=EE^{{\rm T}}={P}_{k|k}-{W}_{k}R_{k}{W}_{k}^{\rm{T}} $
(81) 根据附录中的引理1、方程(76)和方程(81), 那么存在正交矩阵M使得:
$ E\bar{M}=D $
(82) 将方程(82)和B=0代入到方程(74)中, 我们可以得到:
$ \begin{array}{l} G={S_{k|k}}M_3^{\rm{T}}{(E\bar M)^{ - 1}}EM_2^{\rm{T}}S_{k|k - 1}^{ - 1}=\qquad \\ \;\;\;\;\;\;\;{S_{k|k}}{({M_2}\bar M{M_3})^{\rm{T}}}S_{k|k - 1}^{ - 1}={S_{k|k}}{({M_4})^{\rm{T}}}S_{k|k - 1}^{ - 1} \end{array} $
(83) 其中
$ M_{4}=M_{2}\bar{M}M_{3} $
(84) 由于M2, M, M3为正交矩阵, 从而可以得到:
$ M_{4}M_{4}^{{\rm T}}=M_{2}\bar{M}M_{3}M_{3}^{{\rm T}}\bar{M}^{{\rm T}}M_{2}^{{\rm T}}=I $
(85) 通过方程(83)~(85)可以知道M4为正交矩阵, 并且方程(77)和(78)成立.考虑到M2和M3为任意的正交矩阵, 因此M4也为任意的正交矩阵.
注2. 从定理4的结果可以看出, 文献[23]中提出的求积点更新方法仅是本文提出的求积点更新方法在矩阵B=0时的一种特解.因此, 本文所提出的方法更具一般性.
定理4考虑了矩阵B=0这一极端情况.在这一极端情况下, 更新的求积点仅仅是通过状态一步预测求积点的线性变换获得, 它忽略了量测求积点对状态求积点的修正作用, 从而限制了提出高斯近似滤波的估计精度.因此, 通过考虑B ≠ 0情况, 定理4中给出解的性能还能被进一步提高.为此, 接下来我们将给出在B=Wk时的另一种特解.在这种情况下, 矩阵G和H的解将在如下的定理5中给出.
定理5. 如果矩阵 B=Wk, 那么矩阵G和H可以被计算为
$ \begin{align} &G=S_{k|k}M_{3}^{{\rm T}}D^{-1}(W_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+\nonumber\\ &\qquad S_{k|k-1}M_{5}S_{k|k-1}^{-1}-W_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-T}M_{5}S_{k|k-1}^{-1}) \end{align} $
(86) $ H=S_{k|k}M_{3}^{{\rm T}}D^{-1}W_{k} $
(87) 其中, M5为任意的正交矩阵
$ M_{5}M_{5}^{{\rm T}}=I $
(88) 证明. 将B=Wk 代入到方程(75)中, 可以得到:
$ H=S_{k|k}M_{3}^{{\rm T}}D^{-1}W_{k} $
(89) 将B=Wk 代入到方程(57)中, 可以得到:
$ \begin{align} &EE^{{\rm T}}={P}_{k|k}-{W}_{k}{P}_{{\boldsymbol zz}, k|k-1}{W}_{k}^{\rm{T}}+\nonumber\\ &\qquad {W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}{W}_{k}^{\rm{T}} \end{align} $
(90) 利用方程(12), 我们可以得到如下关系式:
$ \begin{align} {W}_{k}{P}_{{\boldsymbol zz}, k|k-1}{W}_{k}^{\rm{T}}={W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}={P}_{{\boldsymbol xz}, k|k-1}{W}_{k}^{\rm{T}} \end{align} $
(91) 将方程(11)和(91)代入到方程(90)中, 可以得到:
$ \begin{align} &EE^{{\rm T}}={P}_{k|k-1}-{W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}-{P}_{{\boldsymbol xz}, k|k-1}{W}_{k}^{\rm{T}}+\nonumber\\ &\ \ {W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}{W}_{k}^{\rm{T}}=S_{k|k-1}S_{k|k-1}^{{\rm T}}- \nonumber\\ &\ \ {W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}}S_{k|k-1}^{{\rm T}}\!-\!S_{k|k-1}S_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}\times \nonumber\\ &\ \ {W}_{k}^{\rm{T}}+{W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}}S_{k|k-1}^{-1}{P}_{{\boldsymbol xz}, k|k-1}{W}_{k}^{\rm{T}}=\nonumber\\ &\ (S_{k|k-1}-{W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})(S_{k|k-1}- \nonumber\\ &\ \ {W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})^{{\rm T}} \end{align} $
(92) 利用方程(11)和方程(91), 矩阵$S_{k|k-1}-{W}_{k}${P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}}$可以被重新表示为
$ \begin{align} &S_{k|k-1}-{W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}}=(P_{k|k-1}- \nonumber\\ &\qquad {W}_{k}{P}_{{\boldsymbol zz}, k|k-1}{W}_{k}^{\rm{T}})S_{k|k-1}^{-{\rm T}}=P_{k|k}S_{k|k-1}^{-{\rm T}} \end{align} $
(93) 将方程(93)代入到方程(92)中, 可以得到:
$ \begin{align} &EE^{{\rm T}}=(S_{k|k-1}-{W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})(S_{k|k-1}- \nonumber\\ &\qquad {W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})^{{\rm T}}=P_{k|k}P_{k|k-1}^{-1}P_{k|k} \end{align} $
(94) 因为Pk|k和Pk|k-1分别是状态的估计误差协方差矩阵和一步预测误差协方差矩阵, 所以一般情况下它们都是正定矩阵, 即
$ P_{k|k}>0, \qquad P_{k|k-1}>0 $
(95) 将方程(95)代入到方程(94)中, 可以得到:
$ \begin{align} &EE^{{\rm T}}=(S_{k|k-1}-{W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})(S_{k|k-1}- \nonumber\\ &\qquad {W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})^{{\rm T}}=P_{k|k}P_{k|k-1}^{-1}P_{k|k}>0 \end{align} $
(96) 根据附录中的引理1和方程(96), 那么存在正交矩阵$\tilde{M}$使得:
$ \begin{align} E=(S_{k|k-1}-{W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})\tilde{M} \end{align} $
(97) 将B=Wk 和方程(97)代入到方程(74)中, 可以得到:
$ \begin{align} &G=S_{k|k}M_{3}^{{\rm T}}D^{-1}(W_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+(S_{k|k-1}- \nonumber\\ &\quad {W}_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{-{\rm T}})\tilde{M}M_{2}^{{\rm T}}S_{k|k-1}^{-1})=\nonumber\\ &\quad S_{k|k}M_{3}^{{\rm T}}D^{-1}(W_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+\nonumber\\ &\quad S_{k|k-1}M_{5} S_{k|k-1}^{-1}-W_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}S_{k|k-1}^{\rm -T}M_{5}S_{k|k-1}^{-1}) \end{align} $
(98) 其中
$ M_{5}=\tilde{M}M_{2}^{{\rm T}} $
(99) 由于$\tilde{M}$, M2为正交矩阵, 可以得到如下方程:
$ M_{5}M_{5}^{{\rm T}}=\tilde{M}M_{2}^{{\rm T}}M_{2}\tilde{M}^{{\rm T}}=I $
(100) 通过方程(98)和方程(100)可以知道M5为正交矩阵, 并且方程(86)和(88)成立.考虑到M2为任意的正交矩阵, 因此M5也为任意的正交矩阵.
定理5给出的特解可以被进一步简化.令正交矩阵M5= I, 并将其代入到方程(86)中, 我们可以得到如下表达式:
$ \begin{align} &G=S_{k|k}M_{3}^{{\rm T}}D^{-1}(W_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+\nonumber\\ &\qquad S_{k|k-1}S_{k|k-1}^{-1}-W_{k}{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}P_{k|k-1}^{-1})=\nonumber\\ &\qquad S_{k|k}M_{3}^{{\rm T}}D^{-1} \end{align} $
(101) 从而$G=S_{k|k}M_{3}^{{\rm T}}D^{-1}$和$H=S_{k|k}M_{3}^{{\rm T}}D^{-1}W_{k}$也为本文所提出方法的一组特解.
2.2 基于求积准则的改进高斯近似滤波
在本节中, 我们将给出本文所提出方法基于求积准则(14)的具体实施.与标准的高斯近似滤波一样, 提出的改进高斯近似滤波方法也由时间更新和量测更新两部分组成.
1)初始化:根据方程(14)中的方法和相应的求积准则(比如无迹变换[9])产生初始后验密度函数$\mathrm{N}({\boldsymbol x}_{0};\hat{\boldsymbol {x}}_{0|0}, {P}_{0|0})$的求积点${\boldsymbol X}_{0}^{(i)+}$和相应的权值ωi.
2)时间更新
a)利用方程(25)传播上一时刻更新的求积点$\boldsymbol{X}_{k - 1}^{(i)+}$, 得到传播后的求积点χk(i).
b)计算状态的一步预测$\hat{\boldsymbol {x}}_{k|k-1}$.
$ \begin{align} \hat{\boldsymbol {x}}_{k|k-1}=\sum\limits_{i=1}^{N}\omega_{i}{\boldsymbol \chi}_{k}^{(i)} \end{align} $
(102) c)利用方程(26)计算传播求积点的误差矩阵$\tilde {\boldsymbol {\chi}}_{k}$.
d)计算状态一步预测误差协方差矩阵Pk|k-1.
$ {P}_{k|k-1}=\tilde{\boldsymbol {\chi}}_{k}W\tilde {\boldsymbol {\chi}}_{k}^{\rm T}+{Q}_{k-1} $
(103) e)利用方程(43)~(45)计算矩阵F, 并利用方程(37)计算状态预测求积点误差矩阵$\tilde {\boldsymbol {X}}_{k}^{-}$.
f)计算一步预测状态的求积点${\boldsymbol X}_{k}^{(i)-}$.
$ \begin{align} [{\boldsymbol X}_{k}^{(1)-}\cdots{\boldsymbol X}_{k}^{(N)-}]=\tilde {\boldsymbol {X}}_{k}^{-}+[\hat{\boldsymbol {x}}_{k|k-1}\cdots\hat{\boldsymbol {x}}_{k|k-1}] \end{align} $
(104) 3)量测更新
a)利用方程(28)计算一步预测量测的求积点Zk (i)-.
b)计算量测的一步预测$\hat{\boldsymbol {z}}_{k|k-1}$.
$ \begin{align} \hat{\boldsymbol {z}}_{k|k-1}=\sum\limits_{i=1}^{N}\omega_{i}{\boldsymbol Z}_{k}^{(i)-} \end{align} $
(105) c)利用方程(29)计算量测预测求积点误差矩阵$\tilde {\boldsymbol {Z}}_{k}^{-}$.
d)计算量测预测误差协方差矩阵Pzz, k|k-1, 状态和量测的互协方差矩阵Pxz, k|k-1.
$ \begin{align} {P}_{{\boldsymbol zz}, k|k-1}=\tilde{\boldsymbol {Z}}_{k}^{-}W(\tilde {\boldsymbol {Z}}_{k}^{-})^{{\rm T}}+{R}_{k} \end{align} $
(106) $ \begin{align} {P}_{{\boldsymbol xz}, k|k-1}=\tilde{\boldsymbol {X}}_{k}^{-}W(\tilde {\boldsymbol {Z}}_{k}^{-})^{{\rm T}} \end{align} $
(107) e)利用方程(12)计算卡尔曼滤波增益Wk, 并利用方程(10)和(11)更新状态估计${\boldsymbol{{\hat x}}_{k|k}}$和相应的估计误差协方差矩阵Pk|k .
f)利用方程(57)和(58), (68)和(69), (74)和(75)计算矩阵G和H, 并利用方程(38)计算状态更新求积点误差矩阵$\boldsymbol{\tilde \xi} _k^+$.
g)计算更新的求积点X k(i)+.
$ \begin{align} [{\boldsymbol X}_{k}^{(1)+}\cdots{\boldsymbol X}_{k}^{(N)+}]=\tilde {\boldsymbol {X}}_{k}^{+}+[\hat{\boldsymbol {x}}_{k|k}\cdots\hat{\boldsymbol {x}}_{k|k}] \end{align} $
(108) 本文所提出的改进高斯近似滤波算法流程如图 1所示.从图 1中可以看到, 提出的方法不需要基于高斯假设重复地产生求积点, 而是直接更新求积点.从图 1中我们还可以看到, 本文所提出的方法只需要在初始化时基于高斯假设产生一次求积点X 0(i)+.基于不同的求积准则产生初始求积点X 0(i)+和相应的权值ωi可以推导出不同的非线性滤波方法.
图 1 提出的改进高斯近似滤波算法的流程图Fig. 1 Diagram of the proposed improved Gaussian approximate filtering algorithm接下来本文将比较所提出的方法和标准高斯近似滤波方法.首先, 提出的方法与标准高斯近似滤波方法采用不同的方法计算状态和量测的一步预测均值$\hat{\boldsymbol {x}}_{k|k-1}$、$\hat{\boldsymbol {z}}_{k|k-1}$和协方差矩阵Pk|k-1、Pzz, k|k-1、Pxz, k|k-1.标准高斯近似滤波方法采用方程(4)~(8)来计算状态和量测的一步预测均值和协方差矩阵, 然而提出的方法采用如下的方法:
$ \begin{align} \hat{\boldsymbol {x}}_{k|k-1}=\int_{\mathbf{R}^{n}}{\boldsymbol f}_{k-1}({\boldsymbol x}_{k-1})p({\boldsymbol x}_{k-1}|{\boldsymbol Z}_{k-1}){\rm d}{{\boldsymbol x}_{k-1}} \end{align} $
(109) $ \begin{align} &{P}_{k|k-1}=\int_{\mathbf{R}^{n}}{\boldsymbol f}_{k-1}({\boldsymbol x}_{k-1}){\boldsymbol f}_{k-1}^{{\rm T}}({\boldsymbol x}_{k-1})\times \nonumber\\ &\qquad p({\boldsymbol x}_{k-1}|{\boldsymbol Z}_{k-1}){\rm d}{{\boldsymbol x}_{k-1}}-\hat{\boldsymbol {x}}_{k|k-1}\hat{\boldsymbol {x}}_{k|k-1}^{{\rm T}}+{Q}_{k-1} \end{align} $
(110) $ \begin{align} \hat{\boldsymbol {z}}_{k|k-1}=\int_{\mathbf{R}^{n}}{\boldsymbol h}_{k}({\boldsymbol x}_{k})p({\boldsymbol x}_{k}|{\boldsymbol Z}_{k-1}){\rm d}{{\boldsymbol x}_{k}} \end{align} $
(111) $ \begin{align} &{P}_{{\boldsymbol zz}, k|k-1}=\int_{\mathbf{R}^{n}}{\boldsymbol h}_{k}({\boldsymbol x}_{k}){\boldsymbol h}_{k}^{{\rm T}}({\boldsymbol x}_{k})p({\boldsymbol x}_{k}|{\boldsymbol Z}_{k-1}){\rm d}{{\boldsymbol x}_{k}}- \nonumber\\ &\qquad \hat{\boldsymbol {z}}_{k|k-1}\hat{\boldsymbol {z}}_{k|k-1}^{{\rm T}}+{R}_{k} \end{align} $
(112) $ \begin{align} &{P}_{{\boldsymbol xz}, k|k-1}=\int_{\mathbf{R}^{n}}{\boldsymbol x}_{k}{\boldsymbol h}_{k}^{{\rm T}}({\boldsymbol x}_{k})p({\boldsymbol x}_{k}|{\boldsymbol Z}_{k-1}){\rm d}{{\boldsymbol x}_{k}}- \nonumber\\ &\qquad \hat{\boldsymbol {x}}_{k|k-1}\hat{\boldsymbol {z}}_{k|k-1}^{{\rm T}} \end{align} $
(113) 其中, 状态后验PDF p(xk-1|Z k-1)和状态一步预测PDF p(xk|Z k-1)表示如下:
$ \begin{align} p({\boldsymbol x}_{k-1}|{\boldsymbol Z}_{k-1})=\sum\limits_{i=1}^{N}\omega_{i}\delta({\boldsymbol x}_{k-1}-{\boldsymbol X}_{k-1}^{(i)+}) \end{align} $
(114) $ \begin{align} p({\boldsymbol x}_{k}|{\boldsymbol Z}_{k-1})=\sum\limits_{i=1}^{N}\omega_{i}\delta({\boldsymbol x}_{k}-{\boldsymbol X}_{k}^{(i)-}) \end{align} $
(115) 其中, $\boldsymbol{X}_{k - 1}^{(i)+}$和${\boldsymbol X}_{k}^{(i)-}$不是基于高斯假设产生的求积点, 而是基于定理1~3直接更新的求积点.
通过比较方程(4)~(8)和方程(109)~(115)可以发现, 标准的高斯近似滤波方法在计算状态和量测的一步预测均值和协方差矩阵时, 需要假设状态后验PDF和状态一步预测PDF都是高斯的, 然而提出的方法不需要这些高斯假设.
其次, 提出的方法与标准高斯近似滤波方法采用不同的方法获得求积点.在状态后验PDF和状态一步预测PDF的高斯假设下, 标准高斯近似滤波方法利用数值技术产生求积点${\boldsymbol \zeta}_{k}^{(i)-}$和${\boldsymbol \zeta}_{k}^{(i)+}$.不失一般性, 我们选择三阶球径容积准则来产生求积点${\boldsymbol \zeta}_{k}^{(i)-}$和${\boldsymbol \zeta}_{k}^{(i)+}$[4].
$ \begin{align} {\boldsymbol \zeta}_{k}^{(i)-}=\hat{\boldsymbol {x}}_{k|k-1}+S_{k|k-1}{\boldsymbol \lambda}_{i} \end{align} $
(116) $ \begin{align} {\boldsymbol \zeta}_{k}^{(i)+}=\hat{\boldsymbol {x}}_{k|k}+S_{k|k}{\boldsymbol \lambda}_{i} \end{align} $
(117) $ \begin{align} {\boldsymbol \lambda}_{i}=\left\{\begin{array}{ll} \sqrt{n}{\boldsymbol e}_{i}, \quad &i=1, \cdots, n \\ -\sqrt{n}{\boldsymbol e}_{i-n}, \quad &i=n+1, \cdots, 2n \end{array} \right. \end{align} $
(118) 其中, ei 表示第 i个元素为1的单位列向量.
然而提出的方法只需要利用状态后验PDF和状态一步预测PDF的均值和协方差矩阵信息直接更新求积点X k(i)-和X k(i)+.利用方程(25)~(27)和方程(37), X k(i)-可以表示如下:
$ \begin{align} &{\boldsymbol X}_{k}^{(i)-}=\hat{\boldsymbol {x}}_{k|k-1}+\tilde{\boldsymbol {X}}_{k}^{(i)-}=\hat{\boldsymbol {x}}_{k|k-1}+F\tilde{\boldsymbol {\chi}}_{k}^{(i)}=\nonumber\\ &\qquad \hat{\boldsymbol {x}}_{k|k-1}+F[{\boldsymbol \chi}_{k}^{(i)}-\hat{\boldsymbol {x}}_{k|k-1}]=\nonumber\\ &\qquad \hat{\boldsymbol {x}}_{k|k-1}+F[{\boldsymbol f}_{k-1}({\boldsymbol X}_{k-1}^{(i)+})-\hat{\boldsymbol {x}}_{k|k-1}] \end{align} $
(119) 其中, $\tilde{\boldsymbol {X}}_{k}^{(i)-}$和$\tilde {\boldsymbol {\chi}}_{k}^{(i)}$分别表示误差矩阵$\tilde {\boldsymbol {X}}_{k}^{-}$和${\boldsymbol{{\tilde \chi }}_k}$的第i列.
将方程(43)代入到方程(119)中, X k(i)-可以被重新表示如下:
$ \begin{align} &{\boldsymbol X}_{k}^{(i)-}=\hat{\boldsymbol {x}}_{k|k-1}+S_{k|k-1}M_{1}^{{\rm T}}(S_{k|k-1}^{1})^{-1}[{\boldsymbol f}_{k-1}\times \nonumber\\ &\qquad({\boldsymbol X}_{k-1}^{(i)+})-\hat{\boldsymbol {x}}_{k|k-1}]=\hat{\boldsymbol {x}}_{k|k-1}+S_{k|k-1}{\boldsymbol \eta}_{i} \end{align} $
(120) 其中, ηi定义如下:
$ \begin{align} {\boldsymbol \eta}_{i}=M_{1}^{{\rm T}}(S_{k|k-1}^{1})^{-1}[{\boldsymbol f}_{k-1}({\boldsymbol X}_{k-1}^{(i)+})-\hat{\boldsymbol {x}}_{k|k-1}] \end{align} $
(121) 利用方程(27)~(29)和方程(38), X k(i)+可以表示如下:
$ \begin{align} &{\boldsymbol X}_{k}^{(i)+}=\hat{\boldsymbol {x}}_{k|k}+\tilde{\boldsymbol {X}}_{k}^{(i)+}=\hat{\boldsymbol {x}}_{k|k}+G\tilde{\boldsymbol {X}}_{k}^{(i)-}-H\tilde{\boldsymbol {Z}}_{k}^{(i)-}=\nonumber\\ &\quad \hat{\boldsymbol {x}}_{k|k}+G[{\boldsymbol X}_{k}^{(i)-}-\hat{\boldsymbol {x}}_{k|k-1}]-H[{\boldsymbol Z}_{k}^{(i)-}-\hat{\boldsymbol {z}}_{k|k-1}]=\nonumber\\ &\quad \hat{\boldsymbol {x}}_{k|k}+G[{\boldsymbol X}_{k}^{(i)-}-\hat{\boldsymbol {x}}_{k|k-1}]-H[{\boldsymbol h}_{k}({\boldsymbol X}_{k}^{(i)-})-\nonumber\\ &\quad \hat{\boldsymbol {z}}_{k|k-1}] \end{align} $
(122) 其中, $\tilde{\boldsymbol {X}}_{k}^{(i)+}$和$\tilde {\boldsymbol {Z}}_{k}^{(i)-}$分别表示误差矩阵$\tilde{\boldsymbol {X}}_{k}^{+}$和$\tilde{\boldsymbol {Z}}_{k}^{-}$的第i列.
将方程(74)和(75)代入到方程(122)中, X k(i)+可以被重新表示如下:
$ \begin{align} &{\boldsymbol X}_{k}^{(i)+}=\hat{\boldsymbol {x}}_{k|k}+S_{k|k}M_{3}^{{\rm T}}D^{-1}\{(B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+\nonumber\\ &\quad EM_{2}^{{\rm T}}S_{k|k-1}^{-1})[{\boldsymbol X}_{k}^{(i)-}-\hat{\boldsymbol {x}}_{k|k-1}]-B[{\boldsymbol h}_{k}({\boldsymbol X}_{k}^{(i)-})-\nonumber\\ &\quad \hat{\boldsymbol {z}}_{k|k-1}]\}=\hat{\boldsymbol {x}}_{k|k}+S_{k|k}{\boldsymbol \theta}_{i} \end{align} $
(123) 其中, θi定义如下:
$ \begin{align} &{\boldsymbol \theta}_{i}=M_{3}^{{\rm T}}D^{-1}\{(B{P}_{{\boldsymbol xz}, k|k-1}^{{\rm T}}{P}_{k|k-1}^{-1}+EM_{2}^{{\rm T}}S_{k|k-1}^{-1})\times \nonumber\\ &\qquad [{\boldsymbol X}_{k}^{(i)-}-\hat{\boldsymbol {x}}_{k|k-1}]-B[{\boldsymbol h}_{k}({\boldsymbol X}_{k}^{(i)-})-\hat{\boldsymbol {z}}_{k|k-1}]\} \end{align} $
(124) 从方程(116)和(117), (120), (123)中可以看到, λi被用于构造求积点ζk(i)-和ζk(i)+, 而ηi 和 θi被分别用于更新求积点X k(i)-和X k(i)+.根据方程(118)可以知道, λi被约束在坐标轴上, 从而使得求积点ζk(i)-和ζk(i)+不能捕获状态一步预测PDF和状态后验PDF的高阶矩信息[24].根据方程(121)和方程(124)可以知道, η i和θ i没有被约束在坐标轴上, 并且它们包含了系统模型f k-1(·)和量测模型hk(·)的非线性信息, 从而使得状态一步预测PDF和状态后验PDF的一些高阶矩信息和非高斯信息被保留在求积点X k(i)-和X k(i)+中.因此, 提出的方法比标准的高斯近似滤波方法能更好地近似状态一步预测PDF和状态后验PDF.
最后, 本文将比较和分析所提出的改进高斯近似滤波方法、现有的改进高斯近似滤波方法[23]、标准高斯近似滤波方法[4]的计算复杂度.在实施提出方法和现有方法时, 计算复杂度包括计算矩阵乘法、Cholesky分解、矩阵逆、系统函数和量测函数所需的浮点运算数[26].提出方法和现有方法的浮点运算总数可以表示如下:
$ \begin{align} &\mathrm{fl}_{PIGA}=\mathcal{O}(12n^3+m^3)+9n^3+3n^2N+2nN^2+\nonumber\\ &\qquad 2nNm+mN^2+Nm^2+7mn^2+6nm^2+\nonumber\\ &\qquad Nn+Nm+nm+NM_{f}+NM_{h} \end{align} $
(125) $ \begin{align} &\mathrm{fl}_{EIGA}=\mathcal{O}(3n^3+m^3)+n^3+2n^2N+nNm+\nonumber\\ &\qquad Nm^2+mn^2+2nm^2+3Nn+2Nm+n^2+\nonumber\\ &\qquad m^2+2nm+NM_{f}+NM_{h} \end{align} $
(126) $ \begin{align} &\mathrm{fl}_{SGA}=\mathcal{O}(2n^3+m^3)+n^2N+nNm+Nm^2+\nonumber\\ &\qquad mn^2+2nm^2+3Nn+2Nm+n^2+m^2+\nonumber\\ &\qquad 2nm+NM_{f}+NM_{h} \end{align} $
(127) 其中, flP IGA、flEIGA、flSGA分别表示实施所提出的改进高斯近似滤波方法、现有的改进高斯近似滤波方法、标准高斯近似滤波方法需要的浮点运算总数, $\mathcal{O}(\cdot)$表示浮点运算的阶数, N表示求积点数, Mf和Mh分别表示计算系统函数f k-1(xk-1)和量测函数hk(xk)需要的浮点运算数.从方程(125)~(127)可以看出, 提出方法和现有方法的浮点运算总数的关系可以表示如下:
$ \mathrm{fl}_{PIGA}>\mathrm{fl}_{EIGA}>\mathrm{fl}_{SGA} $
(128) 根据方程(128)可以知道, 本文所提出的改进高斯近似滤波方法比现有的改进高斯近似滤波方法和标准高斯近似滤波方法都具有更高的计算复杂度.
3. 仿真
本节将通过单变量非平稳增长模型、垂直落体模型、再入飞行器目标跟踪的仿真来验证所提出方法的有效性和与现有方法相比的优越性.考虑到文献[23]没有提出具体的方法更新状态预测求积点, 为了公平比较文献[23]中的方法与本文提出的方法, 我们将使用定理1来更新两种方法的状态预测求积点, 并且参数M1=I.在本节的仿真中, 本文所提出方法的参数选择如下, B=Wk , M3=I, M5=I.文献[23]中的方法可以看成是本文所提出方法在B=0时的特例, 并且它的参数M4=I.为了公平比较本文所提出的方法与现有的方法, 我们必须使用相同的求积准则去实施提出的高斯近似滤波方法和现有的高斯近似方法.在本节仿真中, 我们将使用无迹变换来实施所有方法, 其中自由参数被选择为κ=3 -n, 从而分别得到标准的UKF[9]、现有的改进UKF(提出的UKF(B=0))[23], 提出的改进UKF(B=Wk ).此外为了进一步展示提出方法的优越性, 我们还增加了提出的UKF与现有的标准CKF[4]和标准的2阶DDF[12]的比较, 其中2阶DDF的区间长度被选择为$h=\sqrt{3}$[12].为了表述方便, 在以下的仿真中, 我们将2阶DDF简称为"DDF2".
3.1 单变量非平稳增长模型
单变量非平稳增长模型已经被广泛地用作一个标准问题来验证非线性滤波器的性能, 它的状态空间模型可以表示如下[11, 27]:
$ {x}_k=0.5{x}_{k-1}+25\frac{{x}_{k-1}}{1+{x}_{k-1}^2}+8\cos(1.2k)+{w}_{k-1} $
(129) $ {z}_k=\frac{{x}_{k}^2}{20}+{v}_{k} $
(130) 其中, 系统噪声wk和量测噪声vk是不相关的零均值方差分别为Qk =1和Rk=3的高斯白噪声, 初始状态x0是均值为0.1方差为1的高斯随机变量.在每次运行中, 初始状态估计${\hat{x}}_{0|0}$都是从高斯密度函数N(x0; 0.1, 1)中随机抽取的.所有滤波器都被赋予相同的初始条件, 仿真时间T=10000秒.为了比较提出方法和现有方法的滤波性能, 与文献[11]相同, 在此仿真中我们将选择方根均方误差(Root mean square error, RMSE)作为性能指标, 表示如下:
$ \mathrm{RMSE}_{{x}_{k}}=\sqrt{\frac{1}{{T}}\sum\limits_{{k}=1}^{{T}}({x}_{k}^{l}-{\hat{x}}_{k}^{l})^2} $
(131) 其中, T表示每次Monte Carlo仿真时间, xkl和${\hat{x}}_{k}^{l}$表示第l次Monte Carlo运行的真实的和估计的状态.标准的CKF[4]、标准的DDF2[12]、标准的UKF[9]、现有的改进UKF(等价于提出的UKF(B=0))[23]、提出的UKF(B=Wk )的50次Monte Carlo仿真的RMSE结果如图 2和表 1所示, 以及它们的单步运行时间如表 2所示.注意, 当n+κ= h2并且n=1时, 标准的UKF与标准的DDF2等价[28].
表 1 不同滤波方法的平均RMSETable 1 The averaged RMSEs of different filtering methods滤波器 标准的CKF 标准的DDF2和UKF (n=1时等价) 现有的改进UKF 提出的UKF 平均RMSE 7.417 7.787 7.271 7.009 表 2 不同滤波方法的单步运行时间Table 2 The run time of different filtering methods at single step滤波器 标准的CKF 标准的DDF2和UKF (n=1时等价) 现有的改进UKF 提出的UKF 单步运行时间(秒) 0.21 × 10-3 0.22 × 10-3 0.28 × 10-3 0.32 × 10-3 从图 2和表 1中可以看到, 现有的改进UKF和本文提出的UKF都比标准的CKF、标准的DDF2、标准的UKF具有更小的RMSE, 并且本文所提出的UKF在B=Wk 时, 比在B=0时具有更低的RMSE.从表 2中可以看到, 本文提出的方法比现有的方法需要更多的单步运行时间.因此, 本文提出的方法比现有的方法具有更高的估计精度, 但是具有更高的计算复杂度.
3.2 垂直落体模型
垂直落体模型已经被广泛地研究和用于验证非线性滤波性能.在本节仿真中, 我们将利用雷达的幅值量测来估计垂直落体的高度、速度和弹道系数.垂直落体模型的几何图形如图 3所示, 其中x1(t)和x2(t)分别表示落体的高度和下落速度, r(t)和M分别表示落体与雷达之间的距离和水平距离, Z为雷达的高度.
垂直落体的连续时间动力学方程为[12, 20, 23, 25]
$ \begin{align} \dot{{\boldsymbol x}_{\rm{1}}}(t)=-{\boldsymbol x}_{\rm{2}}(t) \end{align} $
(132) $ \begin{align} \dot{{\boldsymbol x}_{\rm{2}}}(t)=-{\rm{e}}^{-\gamma{\boldsymbol x}_{1}(t)}{\boldsymbol x}_{2}^{2}(t){\boldsymbol x}_{3}(t) \end{align} $
(133) $ \begin{align} \dot{{\boldsymbol x}_{\rm{3}}}(t)=0 \end{align} $
(134) 其中, x3(t)表示弹道系数, γ为已知的常数.从雷达和落体之间的相对几何图可以获得如下的量测模型:
$ \begin{align} {\boldsymbol y}_{{k}}=\sqrt{{M}^{\rm{2}}+({\boldsymbol x}_{\rm{1}, {k}}-{Z})^{\rm{2}}}+{\boldsymbol v}_{k} \end{align} $
(135) vk～N(0, Rk).雷达每1秒测量一次间距.为了消除系统强非线性的影响, 我们使用四阶Runge-Kutta方法对(132)~(134)进行积分.系统仿真时间60秒, 仿真参数如下:
$ \gamma=5\times10^{-\rm{5}}, {Z}=10^{\rm{5}}\rm{ft}, {M}=10^{\rm{5}}\rm{ft}, {R}_{k}=10^{\rm{4}}\rm{ft}^{\rm{2}}; $
系统真实状态初值${ x0=[3×105 2×104 10-3]T仿真中滤波初始值设为$\hat{\boldsymbol {x}}_{\rm{0|0}}=[3.5\times10^{\rm{5}}\;2.5\times10^{\rm{4}}\;3\times10^{\rm{-5}}]^{\rm{T}}$, 协方差矩阵${\boldsymbol P}_{\rm{0|0}}={\rm{diag}}\{10^{\rm{6}}, \ 4\times10^{\rm{6}}, \ 10^{\rm{-4}}\}$.与文献[12, 20]相同, 仿真中采用如下定义的平均绝对值误差比较各种滤波方法:
$ \begin{align} \lambda({k})=\frac{1}{{MC}}\sum\limits_{{n}=1}^{{MC}}|{\boldsymbol x}_{k}^{n}-{\boldsymbol x}_{k|k}^{n}| \end{align} $
(136) 其中, MC为Monte Carlo次数.标准的CKF[4]、标准的DDF2[12]、标准的UKF[9], 现有的改进UKF(等价于提出的UKF(B=0))[23], 提出的UKF(B=Wk )的250次Monte Carlo仿真结果如图 4~图 6和表 3所示, 以及它们的单步运行时间如表 4所示.注意, 在三维情况下标准的UKF(κ=3 -n=0)与标准的CKF等价.此外我们还需要注意, 在此仿真中, 标准的UKF(κ=3 -n)和标准的DDF2(h2=3)具有几乎一致的估计性能, 它们的平均绝对值误差曲线重合.
图 4 不同滤波方法的高度估计平均绝对值误差Fig. 4 Averaged absolute error of the altitude of different filtering methods表 3 不同滤波方法在最后30秒内的平均绝对值误差的均值Table 3 The means of averaged absolute errors of different filtering methods over the last 30 s滤波器 高度估计的平均绝对值
误差的均值（ft)速度估计的平均绝对值
误差的均值（ft/s)弹道系数估计的平均
绝对值误差的均值标准的DDF2 143.701 9.636 5.174 ×10-5 标准的CKF和UKF(n=3时等价） 144.003 9.659 5.182 ×10-5 现有的改进UKF 92.060 5.723 3.087 ×10-5 提出的UKF 73.210 3.499 1.886 ×10-5 表 4 不同滤波方法的单步运行时间Table 4 The run time of different filtering methods at single step滤波器 标准的DDF2 标准的CKF和UKF (n=3时等价） 现有的改进UKF 提出的UKF 单步运行时间（秒） 0.28 × 10-3 0.25 × 10-3 0.35 × 10-3 0.50 × 10-3 从图 4~图 6和表 3中我们可以看到, 现有的改进UKF和本文提出的UKF都比标准的CKF、标准的DDF2、标准的UKF具有更小的平均绝对值误差, 并且本文所提出的UKF在B=Wk 时, 比在B=0时具有更低的平均绝对值误差.从表 4中可以看到, 本文提出的方法比现有的方法需要更多的单步运行时间.因此, 本文提出的方法比现有的方法具有更高的估计精度, 但是具有更高的计算复杂度.
图 5 不同滤波方法的速度估计平均绝对值误差Fig. 5 Averaged absolute error of the velocity of different filtering methods
图 6 不同滤波方法的弹道系数估计平均绝对值误差Fig. 6 Averaged absolute error of the ballistic coefficient of different filtering methods3.3 再入飞行器目标跟踪
在本节中, 我们考虑一个飞行器从太空再入大气层的目标跟踪应用.飞行器在非常高的位置以非常高的速度进入大气层.我们通过使用雷达来测量飞行器相对于雷达的距离和方位, 从而实现对它的实时跟踪.状态向量x ∈ R5由位置(x1, x2), 速度(x3, x4)以及与空气动力相关的一个飞行器系数(x5)组成, 即x=[x1 x2 x3 x4 x5]T.飞行器的动力学方程可以表示如下[10, 29]:
$ \begin{align} \dot{{\boldsymbol x}_{\rm{1}}}(t)={\boldsymbol x}_{\rm{3}}(t) \end{align} $
(137) $ \begin{align} \dot{{\boldsymbol x}_{\rm{2}}}(t)={\boldsymbol x}_{\rm{4}}(t) \end{align} $
(138) $ \begin{align} \dot{{\boldsymbol x}_{\rm{3}}}(t)=D(t){\boldsymbol x}_{\rm{3}}(t)+G(t){\boldsymbol x}_{\rm{1}}(t)+{\boldsymbol w}_{1}(t) \end{align} $
(139) $ \begin{align} \dot{{\boldsymbol x}_{\rm{4}}}(t)=D(t){\boldsymbol x}_{\rm{4}}(t)+G(t){\boldsymbol x}_{\rm{2}}(t)+{\boldsymbol w}_{2}(t) \end{align} $
(140) $ \begin{align} \dot{{\boldsymbol x}_{\rm{5}}}(t)={\boldsymbol w}_{\rm{3}}(t) \end{align} $
(141) 其中, D(t)是与拖力相关的项, G(t)是与引力相关的项, w(t)=[w1(t)w2(t)w3(t)]T为系统噪声.拖力项和引力项可以表示如下:
$ \begin{align} D(t)=\beta(t)\exp\left(\frac{R_{0}-R(t)}{H_{0}}\right)V(t) \end{align} $
(142) $ \begin{align} \beta(t)=\beta(0)\exp{\boldsymbol x}_{\rm{5}}(t), \qquad G(t)=-\frac{Gm_{0}}{R^{3}(t)} \end{align} $
(143) 其中, $R(t)=\sqrt{{\boldsymbol x}_{\rm{1}}^{2}(t)+{\boldsymbol x}_{\rm{2}}^{2}(t)}$为飞行器与地球中心之间的距离, $V(t)=\sqrt{{\boldsymbol x}_{\rm{3}}^{2}(t)+{\boldsymbol x}_{\rm{4}}^{2}(t)}$为飞行器的飞行速率.在此仿真中, 参数设置为β(0)=-0.59783, H0=13.406 km, Gm0=3.9860×105 km3/s2, R0=6374 km.系统噪声 w(t)为零均值高斯白噪声, 其协方差矩阵Q=diag{2.4064×10-6, 2.4064×10-6, 0}.
飞行器的运动被位于(xr, yr)的雷达所观测, 它每隔1秒输出一次飞行器相对于雷达的距离和方位
$ \begin{align} r_{r}(t)=\sqrt{({\boldsymbol x}_{\rm{1}}(t)-x_{r})^{2}+({\boldsymbol x}_{\rm{2}}(t)-y_{r})^{2}}+{\boldsymbol v}_{1}(t) \end{align} $
(144) $ \begin{align} \theta(t)=\mathrm{atan2}({\boldsymbol x}_{\rm{2}}(t)-y_{r}, {\boldsymbol x}_{\rm{1}}(t)-x_{r})+{\boldsymbol v}_{2}(t) \end{align} $
(145) x r=6 374 km, yr =0 km, v(t)=[v1(t)v2(t)]T为量测噪声, v(t)为零均值高斯白噪声, 其协方差矩阵R=diag{(0.02 km)2(0.01 rad)2}.系统真实状态初值x0=[6 500.4 349.14 -1.8093 -6.7967 0.6932]T, 仿真中滤波初始值设为${\hat x}$0|0=[6 499.94 349.11 -1.8091 -6.7962 0.69315]T, 协方差矩阵P 0|0=diag{10-6, 10-6, 10-6, 10-6 1}.为了消除系统强非线性的影响, 我们使用四阶Runge-Kutta方法对(137)~(141)进行积分.为了比较这些滤波器的性能, 我们使用位置、速度和飞行器系数的RMSE作为性能指标.我们定义第t时刻位置的RMSE、速度的RMSE和飞行器系数的RMSE为
$ \begin{align} &\mathrm{RMSE}_{\mathrm{pos}}({t})=1\, 000\times \nonumber\\ &\quad \sqrt{\frac{1}{{MC}}\sum\limits_{{n}=1}^{{MC}}[\left({\boldsymbol x}_{1}^{n}(t)-\hat{\boldsymbol {x}}_{1}^{n}(t)\right)^{2}+\left({\boldsymbol x}_{2}^{n}(t)-\hat{\boldsymbol {x}}_{2}^{n}(t)\right)^{2}]} \nonumber\\ \end{align} $
(146) $ \begin{align} &\mathrm{RMSE}_{\mathrm{vel}}({t})=1\, 000\times \nonumber\\ &\quad \sqrt{\frac{1}{{MC}}\sum\limits_{{n}=1}^{{MC}}[\left({\boldsymbol x}_{3}^{n}(t)-\hat{\boldsymbol {x}}_{3}^{n}(t)\right)^{2}+\left({\boldsymbol x}_{4}^{n}(t)-\hat{\boldsymbol {x}}_{4}^{n}(t)\right)^{2}]} \nonumber\\ \end{align} $
(147) $ \begin{align} \mathrm{RMSE}_{\mathrm{coe}}({t})=\sqrt{\frac{1}{{MC}}\sum\limits_{{n}=1}^{{MC}}\left({\boldsymbol x}_{5}^{n}(t)-\hat{\boldsymbol {x}}_{5}^{n}(t)\right)^{2}} \end{align} $
(148) 其中, MC为Monte Carlo次数, xn(t)和${\boldsymbol{{\hat x}}^n}\left(t \right)$分别表示在第N次Monte Carlo运行时, 第t时刻真实的状态和估计的状态.标准的CKF[4]、标准的DDF2[12]、标准的UKF[9]、现有的改进UKF(等价于提出的UKF(B=0))[23]、提出的UKF(B=Wk )的250次Monte Carlo仿真结果如图 7~图 9和表 5所示, 以及它们的单步运行时间如表 6所示.注意, 在此仿真中, 标准的UKF(κ=3 -n)和标准的DDF2(h2=3)具有几乎一致的估计性能, 它们的RMSE曲线重合.
图 9 不同滤波方法的飞行器系数RMSEFig. 9 RMSEs of the vehicle coefficient of different filtering methods表 5 不同滤波方法在最后25秒内的平均RMSETable 5 The averaged RMSEs of different filtering methods over the last 25 s滤波器 位置的平均RMSE(m) 速度的平均RMSE(m/s) 飞行器系数的平均RMSE 标准的DDF2 12.142 17.023 6.212 × 10-2 标准的CKF 14.577 20.747 8.323 × 10-2 标准的UKF 12.143 17.025 6.213 × 10-2 现有的改进UKF 11.505 16.026 5.601 × 10-2 提出的UKF 9.844 13.332 3.726 × 10-2 表 6 不同滤波方法的单步运行时间Table 6 The run time of different filtering methods at single step滤波器 标准的DDF2 标准的CKF 标准的UKF 现有的改进UKF 提出的UKF 单步运行时间（秒） 1.08 × 10-3 0.97 × 10-3 1.02 × 10-3 1.16 × 10-3 1.20 × 10-3 从图 7~图 9和表 5中我们可以看到, 现有的改进UKF和本文提出的UKF都比标准的CKF、标准的DDF2、标准的UKF具有更小的RMSE, 并且本文所提出的UKF在B=Wk 时, 比在B=0时具有更低的RMSE.从图 7~图 9中我们还可以看到, 本文所提出的方法比现有的方法具有更快的收敛速度.从表 6中可以看到, 本文提出的方法比现有的方法需要更多的单步运行时间.因此, 本文提出的方法比现有的方法具有更高的估计精度和更快的收敛速度, 但是具有更高的计算复杂度.
4. 结论
本文提出了一种高斯近似滤波方法, 推导了它的一般解和特殊解, 并证明了现有的改进高斯近似滤波方法是提出的方法的一种特例.提出的方法比现有的方法能更好地捕获状态一步预测PDF和状态后验PDF的非高斯信息和高阶矩信息.此外, 提出的方法不仅适用于确定的系统模型而且还适用于随机的系统模型.仿真结果表明提出的高斯近似滤波方法比现有的方法具有更高的估计精度.
附录A
引理1. 如果矩阵N1, N2和V满足如下关系式:
$ N_{1}N_{1}^{{\rm T}}=N_{2}N_{2}^{{\rm T}}=V $
(A1) $ V{\rm{ > 0}} $
(A2) 其中, 方程(A2)表示矩阵V是正定的.那么存在正交矩阵O, 使得矩阵N1和N2满足如下方程
$ N_{1}=N_{2}O, \quad OO^{{\rm T}}=I $
(A3) 其中, I为单位矩阵.
证明. 因为正定矩阵的行列式大于0, 所以根据方程(A2), 可以得到:
$ V{\rm{| > 0}} $
(A4) 在方程(A1)两边同时取行列式并利用行列式乘法性质, 可以得到:
$ |N_{1}||N_{1}^{{\rm T}}|=|N_{2}||N_{2}^{{\rm T}}|=|V| $
(A5) 考虑到转置不会改变矩阵的行列式, 可以得到:
$ |N_{1}|=|N_{1}^{{\rm T}}| \quad |N_{2}|=|N_{2}^{{\rm T}} $
(A6) 将方程(A6)代入到方程(A5)中并利用方程(A4), 可以得到:
$ |N_{1}|^{2}=|N_{2}|^{2}=|V|>0 $
(A7) 根据方程(A7), 我们可以知道矩阵N1, N1 T, N2和N2 T都是可逆的.从而可以在方程(A1)两边右乘矩阵(N1 T)-1, 得到如下方程:
$ N_{1}=N_{2}N_{2}^{{\rm T}}(N_{1}^{{\rm T}})^{-1}=N_{2}O $
(A8) 其中, 矩阵O为
$ O=N_{2}^{{\rm T}}(N_{1}^{{\rm T}})^{-1} $
(A9) 考虑到矩阵N1, N1 T, N2和N2 T都是可逆的并利用方程(A1)和(A9), OOT可以被计算如下:
$ \begin{array}{l} O{O^{\rm{T}}}=N_2^{\rm{T}}{(N_1^{\rm{T}})^{ - 1}}N_1^{ - 1}{N_2}=N_2^{\rm{T}}{({N_1}N_1^{\rm{T}})^{ - 1}}{N_2} \times \\ \qquad N_2^{\rm{T}}{({N_2}N_2^{\rm{T}})^{ - 1}}{N_2}=N_2^{\rm{T}}{(N_2^{\rm{T}})^{ - 1}}N_2^{ - 1}{N_2}=I \end{array} $
(A10) 根据方程(A10), 我们可以知道O为正交矩阵.因此如果矩阵N1, N2和V满足方程(A1)和(A2), 那么存在正交矩阵O使得矩阵N1和N2满足方程(A3).
-
图 1 提出的改进高斯近似滤波算法的流程图
Fig. 1 Diagram of the proposed improved Gaussian approximate filtering algorithm
图 4 不同滤波方法的高度估计平均绝对值误差
Fig. 4 Averaged absolute error of the altitude of different filtering methods
图 5 不同滤波方法的速度估计平均绝对值误差
Fig. 5 Averaged absolute error of the velocity of different filtering methods
图 6 不同滤波方法的弹道系数估计平均绝对值误差
Fig. 6 Averaged absolute error of the ballistic coefficient of different filtering methods
图 9 不同滤波方法的飞行器系数RMSE
Fig. 9 RMSEs of the vehicle coefficient of different filtering methods
表 1 不同滤波方法的平均RMSE
Table 1 The averaged RMSEs of different filtering methods
滤波器 标准的CKF 标准的DDF2和UKF (n=1时等价) 现有的改进UKF 提出的UKF 平均RMSE 7.417 7.787 7.271 7.009 
下载: 导出CSV
表 2 不同滤波方法的单步运行时间
Table 2 The run time of different filtering methods at single step
滤波器 标准的CKF 标准的DDF2和UKF (n=1时等价) 现有的改进UKF 提出的UKF 单步运行时间(秒) 0.21 × 10-3 0.22 × 10-3 0.28 × 10-3 0.32 × 10-3
下载: 导出CSV
表 3 不同滤波方法在最后30秒内的平均绝对值误差的均值
Table 3 The means of averaged absolute errors of different filtering methods over the last 30 s
滤波器 高度估计的平均绝对值
误差的均值（ft)速度估计的平均绝对值
误差的均值（ft/s)弹道系数估计的平均
绝对值误差的均值标准的DDF2 143.701 9.636 5.174 ×10-5 标准的CKF和UKF(n=3时等价） 144.003 9.659 5.182 ×10-5 现有的改进UKF 92.060 5.723 3.087 ×10-5 提出的UKF 73.210 3.499 1.886 ×10-5
下载: 导出CSV
表 4 不同滤波方法的单步运行时间
Table 4 The run time of different filtering methods at single step
滤波器 标准的DDF2 标准的CKF和UKF (n=3时等价） 现有的改进UKF 提出的UKF 单步运行时间（秒） 0.28 × 10-3 0.25 × 10-3 0.35 × 10-3 0.50 × 10-3
下载: 导出CSV
表 5 不同滤波方法在最后25秒内的平均RMSE
Table 5 The averaged RMSEs of different filtering methods over the last 25 s
滤波器 位置的平均RMSE(m) 速度的平均RMSE(m/s) 飞行器系数的平均RMSE 标准的DDF2 12.142 17.023 6.212 × 10-2 标准的CKF 14.577 20.747 8.323 × 10-2 标准的UKF 12.143 17.025 6.213 × 10-2 现有的改进UKF 11.505 16.026 5.601 × 10-2 提出的UKF 9.844 13.332 3.726 × 10-2
下载: 导出CSV
表 6 不同滤波方法的单步运行时间
Table 6 The run time of different filtering methods at single step
滤波器 标准的DDF2 标准的CKF 标准的UKF 现有的改进UKF 提出的UKF 单步运行时间（秒） 1.08 × 10-3 0.97 × 10-3 1.02 × 10-3 1.16 × 10-3 1.20 × 10-3
下载: 导出CSV
-
[1] 张勇刚, 黄玉龙, 赵琳.一种带多步随机延迟量测高斯滤波器的一般框架解.自动化学报, 2015, 41(1):122-135 http://www.aas.net.cn/CN/Y2015/V41/I1/122Zhang Yong-Gang, Huang Yu-Long, Zhao Lin. A general framework solution to Gaussian filter with multiple-step randomly-delayed measurements. Acta Automatica Sinica, 2015, 41(1):122-135 http://www.aas.net.cn/CN/Y2015/V41/I1/122 [2] 张勇刚, 黄玉龙, 李宁, 赵琳.带一步随机延迟量测非线性序列贝叶斯估计的条件后验克拉美罗下界.自动化学报, 2015, 41(3):559-574 doi: 10.16383/j.aas.2015.c140391Zhang Yong-Gang, Huang Yu-Long, Li Ning, Zhao Lin. Conditional posterior Cramér-Rao lower bound for nonlinear sequential Bayesian estimation with one-step randomly delayed measurements. Acta Automatica Sinica, 2015, 41(3):559-574 doi: 10.16383/j.aas.2015.c140391 [3] Lei M, van Wyk B J, Qi Y. Online estimation of the approximate posterior Cramer-Rao lower bound for discrete-time nonlinear filtering. IEEE Transactions on Aerospace and Electronic systems, 2011, 47(1):37-57 doi: 10.1109/TAES.2011.5705658 [4] Arasaratnam I, Haykin S. Cubature Kalman filters. IEEE Transactions on Automatic Control, 2009, 54(6):1254-1269 doi: 10.1109/TAC.2009.2019800 [5] Gordon N J, Salmond D J, Smith A F M. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. IEE Proceedings F:Radar and Signal Processing, 1993, 140(2):107-113 doi: 10.1049/ip-f-2.1993.0015 [6] Guo D, Wang X D. Quasi-Monte Carlo filtering in nonlinear dynamic systems. IEEE Transactions on Signal Processing, 2006, 54(6):2087-2098 doi: 10.1109/TSP.2006.873585 [7] Wang X X, Liang Y, Pan Q, Zhao C H. Gaussian filter for nonlinear systems with one-step randomly delayed measurements. Automatica, 2013, 49(4):976-986 doi: 10.1016/j.automatica.2013.01.012 [8] Arasaratnam I, Haykin S, Elliott R J. Discrete-time nonlinear filtering algorithms using Gauss-Hermite quadrature. Proceedings of the IEEE, 2007, 95(5):953-977 doi: 10.1109/JPROC.2007.894705 [9] Julier S J, Uhlman J K, Durrant-Whyte H F. A new method for the nonlinear transformation of means and covariances in filters and estimators. IEEE Transactions on Automatic Control, 2000, 45(3):477-482 doi: 10.1109/9.847726 [10] Julier S J, Uhlman J K. Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 2004, 92(3):401-422 doi: 10.1109/JPROC.2003.823141 [11] Wu Y X, Hu D W, Wu M P, Hu X P. A numerical-integration perspective on Gaussian filters. IEEE Transactions on Signal Processing, 2006, 54(8):2910-2921 doi: 10.1109/TSP.2006.875389 [12] Norgaard M, Poulsen N K, Ravn O. New developments in state estimation for nonlinear systems. Automatica, 2000, 36(11):1627-1638 doi: 10.1016/S0005-1098(00)00089-3 [13] Ito K, Xiong K Q. Gaussian filters for nonlinear filtering problems. IEEE Transactions on Automatic Control, 2000, 45(5):910-927 doi: 10.1109/9.855552 [14] Jia B, Xin M, Cheng Y. High-degree cubature Kalman filter. Automatica, 2013, 49(2):510-518 doi: 10.1016/j.automatica.2012.11.014 [15] Jia B, Xin M, Cheng Y. Sparse-grid quadrature nonlinear filtering. Automatica, 2012, 48(2):327-341 doi: 10.1016/j.automatica.2011.08.057 [16] Duník J, Straka O, Šimandl M. Stochastic integration filter. IEEE Transactions on Automatic Control, 2013, 58(6):1561-1566 doi: 10.1109/TAC.2013.2258494 [17] Zhang X C. A novel cubature Kalman filter for nonlinear state estimation. In:Proceedings of the 2013 IEEE 52nd Annual Conference on Decision and Control. Firenze:IEEE, 2013. 7797-7802 [18] Zhang Y G, Huang Y L, Li N, Zhao L. Embedded cubature Kalman filter with adaptive setting of free parameter. Signal Processing, 2015, 114:112-116 doi: 10.1016/j.sigpro.2015.02.022 [19] Wang S Y, Feng J C, Tse C K. Spherical simplex-radial cubature Kalman filter. IEEE Signal Processing Letters, 2014, 21(1):43-46 doi: 10.1109/LSP.2013.2290381 [20] 张勇刚, 黄玉龙, 武哲民, 李宁.一种高阶无迹卡尔曼滤波方法.自动化学报, 2014, 40(5):838-848 http://www.aas.net.cn/CN/Y2014/V40/I5/838Zhang Yong-Gang, Huang Yu-Long, Wu Zhe-Min, Li Ning. A high order unscented Kalman filtering method. Acta Automatica Sinica, 2014, 40(5):838-848 http://www.aas.net.cn/CN/Y2014/V40/I5/838 [21] Chang L B, Hu B Q, Li A, Qin F J. Transformed unscented Kalman filter. IEEE Transactions on Automatic Control, 2013, 58(1):252-257 doi: 10.1109/TAC.2012.2204830 [22] Zhang Y G, Huang Y L, Li N, Zhao L. Interpolatory cubature Kalman filters. IET Control Theory and Applications, 2015, 9(11):1731-1739 doi: 10.1049/iet-cta.2014.0873 [23] Tian Y, Cheng Y. Novel measurement update method for quadrature-based Gaussian filters. In:Proceedings of the 2013 AIAA Guidance, Navigation, and Control Conference. Boston:AIAA, 2013. 1-15 [24] van der Merwe R. Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models[Ph., D. dissertation], Oregon Health & Science University, Portland, OR, USA, 2004 [25] Cheng Y, Tian Y, Crassidis J L. Extension of the sparse grid quadrature filter. In:Proceedings of the 17th International Conference on Information Fusion (FUSION). Salamanca:IEEE, 2014. 1-7 [26] Golub G H, Van Loan C F. Matrix Computations (Fourth edition). Baltimore, Maryland:The Johns Hopkins University Press, 2013. [27] 黄玉龙, 张勇刚, 李宁, 赵琳.一种带有色量测噪声的非线性系统辨识方法.自动化学报, 2015, 41(11):1877-1892 http://www.aas.net.cn/CN/Y2015/V41/I11/1877Huang Yu-Long, Zhang Yong-Gang, Li Ning, Zhao Lin. An identification method for nonlinear systems with colored measurement noise. Acta Automatica Sinica, 2015, 41(11):1877-1892 http://www.aas.net.cn/CN/Y2015/V41/I11/1877 [28] Šimandl M, Duník J. Derivative-free estimation methods:new results and performance analysis. Automatica, 2009, 45(7):1749-1757 [29] Lee J H. Nonlinear estimation and multiple sensor fusion using unscented information filtering. IEEE Signal Processing Letters, 2008, 15:861-864 doi: 10.1109/LSP.2008.2005447 期刊类型引用(11)
1. 李霄，陈锡锻. 一种基于改进的无迹卡尔曼滤波跟踪方法分析. 集成电路应用. 2024(06): 18-19 . 
百度学术2. 崔冰波，吉峰，孙宇，魏新华. 高斯过程改进的鲁棒容积卡尔曼滤波及其组合导航应用. 电子测量与仪器学报. 2021(09): 34-40 . 百度学术3. 丁小奇，李健，胡雅婷，史中元，任虹宾，陈营华. 基于改进SURF算法无人机影像特征匹配的研究. 中国农机化学报. 2020(02): 147-154 . 百度学术4. 姬广奥，刘志强. 一种基于Edline线特征的车道线识别算法. 河北工业科技. 2020(03): 190-195 . 百度学术5. 刘颖，李钊，公衍超，林庆帆，王富平. 现场勘验刀具图像特征描述与识别. 西安邮电大学学报. 2020(01): 49-55 . 百度学术6. 李健，丁小奇，陈光，孙旸，姜楠. 基于改进高斯滤波算法的叶片图像去噪方法. 南方农业学报. 2019(06): 1385-1391 . 百度学术7. 柯铭，张天明，秦爱景，王波，白旭. 灰度极值加权求和图像振铃效应评价算法. 哈尔滨理工大学学报. 2019(05): 93-100 . 百度学术8. 乔少杰，韩楠，丁治明，金澈清，孙未未，舒红平. 多模式移动对象不确定性轨迹预测模型. 自动化学报. 2018(04): 608-618 . 本站查看9. 栾松宇，周文举，孔清清，郭鹏，徐宗鑫. 基于深搜算法的手机外壳刮擦痕迹检测. 鲁东大学学报(自然科学版). 2018(04): 309-313+326 . 百度学术10. 郑婷婷，杨旭升，张文安，俞立. 一种高斯渐进滤波框架下的目标跟踪方法. 自动化学报. 2018(12): 2250-2258 . 本站查看11. 黄玉龙，张勇刚，武哲民，李宁，王刚. 带有色厚尾量测噪声的鲁棒高斯近似滤波器和平滑器. 自动化学报. 2017(01): 114-131 . 本站查看其他类型引用(24)
-
下载:
计量
- 文章访问数: 3388
- HTML全文浏览量: 220
- PDF下载量: 1319
- 被引次数: 35
