留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

基于后向差分Delta算子的卡尔曼滤波算法及其仿真

宋召青 王康

宋召青, 王康. 基于后向差分Delta算子的卡尔曼滤波算法及其仿真[J]. 电子科技大学学报, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010
引用本文: 宋召青, 王康. 基于后向差分Delta算子的卡尔曼滤波算法及其仿真[J]. 电子科技大学学报, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010
SONG Zhao-Qing, WANG Kang. Kalman Filter Algorithm Based on Backward Differentiation Formula of Delta Operator with Simulation[J]. Journal of University of Electronic Science and Technology of China, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010
Citation: SONG Zhao-Qing, WANG Kang. Kalman Filter Algorithm Based on Backward Differentiation Formula of Delta Operator with Simulation[J]. Journal of University of Electronic Science and Technology of China, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010

基于后向差分Delta算子的卡尔曼滤波算法及其仿真

doi: 10.3969/j.issn.1001-0548.2016.05.010
基金项目: 

国家自然科学基金重点项目 61433011

山东省优秀中青年科学家科研奖励基金 BS2012DX007

上海市博士后科研计划 12R21414300

详细信息
    作者简介:

    宋召青(1969-),男,博士,教授,主要从事控制理论及应用方面的研究

  • 中图分类号: O23

Kalman Filter Algorithm Based on Backward Differentiation Formula of Delta Operator with Simulation

图(4)
计量
  • 文章访问数:  3957
  • HTML全文浏览量:  1117
  • PDF下载量:  162
  • 被引次数: 0
出版历程
  • 收稿日期:  2015-04-16
  • 修回日期:  2015-09-30
  • 刊出日期:  2016-09-01

基于后向差分Delta算子的卡尔曼滤波算法及其仿真

doi: 10.3969/j.issn.1001-0548.2016.05.010
    基金项目:

    国家自然科学基金重点项目 61433011

    山东省优秀中青年科学家科研奖励基金 BS2012DX007

    上海市博士后科研计划 12R21414300

    作者简介:

    宋召青(1969-),男,博士,教授,主要从事控制理论及应用方面的研究

  • 中图分类号: O23

摘要: 在高速采样时,Delta算子离散化模型的参数趋于原来的连续时间模型,且Delta算子实现时系统具有较好的数字特性,使其能够改善基于传统移位算子的离散卡尔曼滤波中存在的滤波发散问题。该文采用正交投影法,推导了基于Delta算子的随机线性离散系统的卡尔曼滤波方程,最终给出了基于后向差分Delta算子的卡尔曼滤波递推算法,并进行了仿真研究。实验结果和性能分析表明,在高频采样情形下基于后向差分Delta算子的递推卡尔曼滤波的收敛性能优于常规卡尔曼滤波。

English Abstract

宋召青, 王康. 基于后向差分Delta算子的卡尔曼滤波算法及其仿真[J]. 电子科技大学学报, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010
引用本文: 宋召青, 王康. 基于后向差分Delta算子的卡尔曼滤波算法及其仿真[J]. 电子科技大学学报, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010
SONG Zhao-Qing, WANG Kang. Kalman Filter Algorithm Based on Backward Differentiation Formula of Delta Operator with Simulation[J]. Journal of University of Electronic Science and Technology of China, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010
Citation: SONG Zhao-Qing, WANG Kang. Kalman Filter Algorithm Based on Backward Differentiation Formula of Delta Operator with Simulation[J]. Journal of University of Electronic Science and Technology of China, 2016, 45(5): 767-771. doi: 10.3969/j.issn.1001-0548.2016.05.010
  • 随着计算机和自动化技术的飞速发展,现今的自动控制系统的设计思路都来源于数字计算机的广泛应用,为了呈现出系统特性,必须对系统的离散时间进行描述。因此,人类对高速采样情形下信号传输和处理问题的需求日益增加[1-2]。然而,当采用传统的移位算子处理高速采样系统时,计算机有限字长将会引起采样系统量化误差以及极限环振荡等数值不稳定问题[3],且易于引入非最小相位零点,导致离散化系统稳定性能降低。为此结合随机线性系统Kalman滤波基本方程[4-5],文献[6]采用Delta算子方法对随机线性连续系统进行离散化,有效解决了上述问题。

    Delta算子是一种新的离散化方法,现已成为连续时间模型和离散时间模型的统一描述方法,用它来描述离散系统有如下优点[7]:1) 在高频采样情况下,能使离散化模型趋近于连续模型。2) Delta算子实现时具有较好的数字特性,包括有限字长特性和系数灵敏度等。3) Delta算子模型中易于观察分析不同采样周期下系统性能。另外,基于传统移位算子的离散卡尔曼滤波模型存在对模型噪声和量测噪声的统计特性认识不足导致的方差取值不合适以及计算机有限字长导致的舍入误差累计等问题,最终出现滤波发散现象[8]。因此将Delta算子引入卡尔曼滤波中进行研究[9-10],推导新的滤波算法,有效改善系统在高速采样情况下的滤波发散问题。

    文献[11]给出了Delta算子的前向差分和后向差分的定义,本文分别分析了基于前向差分和后向差分Delta算子的递推卡尔曼滤波方法的合理性,最终推导了基于后向差分Delta算子的递推卡尔曼滤波。

    • Delta算子的前向差分定义为:

      $$\delta {\rm{ = }}\frac{{q - 1}}{T}$$ (1)

      Delta算子的后向差分定义为:

      $$\delta {\rm{ = }}\frac{{1 - {q^{ - 1}}}}{T}$$ (2)

      式中,q为前向移位算子,满足$qf(k) = f(k + 1)$;T为系统采样周期,当T越接近0时,离散系统越接近连续系统。

    • 假设XZ分别为具有二阶矩的n维和m维随机向量,如果存在一个与X同维的随机向量$\hat X$,满足:1) $\hat X$可以由Z线性表示;2) 无偏性;3) $X - \hat X$与Z正交,则称$\hat X$是XZ上的正交投影,记为$\hat X = \hat E[X/Z]$。

      向量正交投影具有下列结论[4]

      1)$\hat E[X/Z] = EX + {\mathop{\rm cov}} [X,Z]{({\rm{Var}}Z)^{ - 1}}[Z - EZ]$;

      2)$\hat E[AX/Z] = A\hat E[X/Z]$;

      3)$\hat E[(AX + BY)/Z] = A\hat E[X/Z] + B\hat E[Y/Z]$;

      4) 如果$Z = [{Z_1};{Z_2}]$,则有:

      $\hat E[X/Z] = \hat E[X/{Z_1}] + \hat E[\tilde X/{\tilde Z_2}]{\rm{ = }}$

      $\hat E[X/{Z_1}] + \hat E[\tilde X\tilde Z_2^{\rm{T}}]{[E[{\tilde Z_2}\tilde Z_2^{\rm{T}}]]^{ - 1}}{\tilde Z_2}$

      式中,$\tilde X = X - \hat E[X/{Z_1}],{\rm{ }}{\tilde Z_2} = {Z_2} - \hat E[{Z_2}/Z]$。

    • 考虑如下形式的连续模型[12]

      $$\dot X(t) = AX(t) + FW(t)$$ (3)
      $$Z(t) = CX(t) + V(t)$$ (4)

      式中,X(t)为系统n维状态向量;X(t)为系统m维观测向量;X(t)是m维系统随机观测噪声向量;X(t)是p维系统随机过程噪声向量;An×n维常值矩阵;Fn×p维常值矩阵;Cm×n维常值矩阵。

    • 通过零阶保持器离散化(采样周期为T),那么相应的Z算子形式的离散模型为:

      $$X(k + 1) = {A_z}X(k) + {W_z}(k)$$ (5)
      $$Z(k) = {C_z}X(k) + {V_z}(k)$$ (6)

      式中,系统系数矩阵满足${A_z} = {{\rm{e}}^{AT}}$;${C_z} = C$;${V_z}(k)$是系统的观测噪声序列;${W_z}(k)$是过程噪声序列,满足[5]

      $$\left\{ \begin{array}{l} {W_z}(k) = \int_{{\rm{ }}0}^{{\rm{ }}T} {{{\rm{e}}^{A(T - t)}}FW(t)} {\rm{d}}t\\ E[{W_z}(k)W_z^{\rm{T}}(k + i)] = 0(\forall i \ne 0) \end{array} \right.$$ (7)

      定义系统过程噪声向量序列${W_z}(k)$的方差阵$Q(k) = E[{W_z}(k)W_z^{\rm{T}}(k)]$,系统观测噪声向量序列${V_z}(k)$的方差阵$R(k) = E[{V_z}(k)V_z^{\rm{T}}(k)]$。

    • 根据$\delta $算子后向差分定义,可得基于后向差分$\delta $域内的离散系统为:

      $$\delta X(k + 1) = {A_\delta }X(k) + {W_\delta }(k)$$ (8)
      $$Z(k) = {C_\delta }X(k) + {V_\delta }(k)$$ (9)

      式中,

      $$\left\{ \begin{array}{l} {A_\delta } = ({A_z} - I)/T\\ {W_\delta }(k) = {W_z}(k)/T\\ {Q_\delta }(k) = {\rm{Var}}[{W_\delta }(k)] = \frac{1}{{{T^2}}}Q(k)\\ {C_\delta } = {C_z} = C\\ {V_\delta }(k) = {V_z}(k) \end{array} \right.$$ (10)
    • 根据$\delta $算子前向差分定义,可得基于前向差分$\delta $域内的离散系统为:

      $$\delta X(k) = {A_\delta }X(k) + {W_\delta }(k)$$ (11)
      $$Z(k) = {C_\delta }X(k) + {V_\delta }(k)$$ (12)

      可见,两者的区别在于系统的状态方程不同,而观测方程保持一致。

    • 由基于前向差分Delta域的离散化系统状态方程式(11)和量测方程式(12),对于给定的观测空间${Z_k}$,根据正交投影的定义可得:

      $$\delta \hat X(k) = \hat E[\delta X(k)|{Z_k}]$$ (13)

      所以由正交投影的结论2)和结论3)得:

      $$\begin{array}{l} T\delta \hat X(k) = \hat E[T\delta X(k)|{Z_k}] = \\ \hat E[(X(k + 1) - X(k))|{Z_k}] = \\ \hat E[X(k + 1)|{Z_k}] - \hat E[X(k)|{Z_k}] = \\ \hat X(k + 1|k) - \hat X(k) \end{array}$$ (14)

      同时根据Delta算子前向差分定义有:

      $$T\delta \hat X(k) = \hat X(k + 1) - \hat X(k)$$ (15)

      比较式(14)和式(15)可得:

      $$\hat X(k + 1|k) = \hat X(k + 1)$$ (16)

      式(16)表示在时域中,基于前向差分Delta算子的状态滤波值和状态预测估计值无法分辨,所以采用后向差分的Delta算子的状态差分方程式(8)和量测方程式(9)推导基于Delta算子的卡尔曼滤波递推算法。

    • 1) 基于Delta算子的滤波方程。

      根据正交投影定义以及其结论,可得$\delta \hat X(k + 1)$的递推公式为:

      $$\begin{array}{l} \delta \hat X(k + 1) = E[\delta X(k + 1)|{Z_{k + 1}}]{\rm{ = }}\\ E[\delta X(k + 1)|{Z_k},\tilde Z(k + 1|k)]{\rm{ = }}\\ E[\delta {\bf{X}}(k + 1)|{{\bf{Z}}_k}] + E[\delta {\bf{X}}(k + 1)|{\bf{\tilde Z}}(k + 1|k)]{\rm{ = }}\\ \delta \hat X(k + 1|k) + {K_\delta }(k + 1){\bf{\tilde Z}}(k + 1|k) \end{array}$$ (17)

      式中,基于k次观测向量集合为:

      $${Z_k} = [Z(1),Z(2), \cdots ,Z(k)]$$ (18)

      ${\bf{\tilde Z}}(k + 1|k)$为新息,表示从第k+1次观测量Z(k+1)中减去前k次观测量中所得到的Z(k+1)的预报值$\hat Z(k)$。并定义滤波增益矩阵为:

      $$\begin{array}{l} {K_\delta }(k + 1) = \\ {\rm{Cov}}[\delta X(k + 1),\tilde Z(k + 1|k)] \cdot {\rm{Va}}{{\rm{r}}^{ - 1}}[\tilde Z(k + 1|k)] \end{array}$$ (19)

      2) 新息及新息方差阵。

      由以上新息的概念可知:

      $$\begin{array}{l} {\bf{\tilde Z}}(k + 1|k) = {\bf{Z}}(k + 1) - {\bf{\hat Z}}(k + 1|k) = \\ Z(k + 1) - E[Z(k + 1)|{Z_k}] = \\ Z(k + 1) - E[{C_\delta }X(k + 1) + {V_\delta }(k + 1)|{Z_k}] = \\ {C_\delta }X(k + 1) + {V_\delta }(k + 1) - {C_\delta }\hat X(k + 1|k) = \\ {C_\delta }\tilde X(k + 1|k) + {V_\delta }(k + 1) \end{array}$$ (20)

      故可以得到新息方差阵为:

      $$\begin{array}{l} {\rm{Var}}[{\bf{\tilde Z}}(k + 1|k)] = {\rm{Var}}[{{\bf{C}}_\delta }{\bf{\tilde X}}(k + 1|k) + {{\bf{V}}_\delta }(k + 1)] = \\ {\rm{ }}{{\rm{C}}_\delta }{\rm{P}}(k + 1|k){\rm{C}}_\delta ^{\mathop{\rm T}\nolimits} + {{\rm{R}}_\delta }(k + 1) \end{array}$$ (21)

      式中,$P(k + 1|k) = {\rm{Var}}[\tilde X(k + 1|k)]$为状态预测误差方差阵;${R_\delta }(k + 1)$为观测噪声向量序列方差阵。

      3) 基于Delta算子的一步最优预测。

      基于${Z_k}$估计$\delta X(k + 1)$而得到的一步最优线性预测为:

      $$\begin{array}{l} \delta \hat X(k + 1|k) = E[\delta X(k + 1)|{Z_k}] = \\ E[{A_\delta }X(k) + {W_\delta }(k)|{Z_k}] = {A_\delta }\hat X(k) \end{array}$$ (22)

      4) 基于Delta算子的预测误差方差阵。

      Delta状态预测误差为:

      $$\delta \tilde X(k + 1|k) = \delta X(k + 1) - \delta \hat X(k + 1|k)$$ (23)

      故其方差阵可表示为:

      $$\begin{array}{l} {P_\delta }(k + 1|k) = {\rm{Var}}[\delta \tilde X(k + 1|k)] = \\ {\rm{Var}}[\delta X(k + 1) - \delta \hat X(k + 1|k)] = \\ {\rm{Var}}[{A_\delta }X(k) + {W_\delta }(k) - {A_\delta }\hat X(k)] = \\ {A_\delta }P(k)A_\delta ^{\rm{T}} + {Q_\delta }(k) \end{array}$$ (24)

      式中,$P(k) = {\rm{Var}}[\tilde X(k)]$;${Q_\delta }(k)$表示过程噪声向量序列方差阵。

      5) 滤波方程的预测。

      由Delta算子的定义以及正交投影的定义和结论,对Delta状态滤波方程进行预测,有:

      $$\begin{array}{l} \hat X(k + 1) = E[X(k) + T\delta X(k + 1)|{Z_{k + 1}}] = \\ E[X(k)|{Z_{k + 1}}] + T\delta \hat X(k + 1) = \\ E[X(k)|{Z_k},\tilde Z(k + 1|k)] + T\delta \hat X(k + 1) = \\ \hat X(k) + E[X(k)|\tilde Z(k + 1|k)] + T\delta \hat X(k + 1) = \\ \hat X(k) + T\delta \hat X(k + 1) + {K_d}(k + 1)\tilde Z(k + 1|k) \end{array}$$ (25)

      式中,

      $$\begin{array}{l} {K_d}(k + 1) = \\ {\rm{Cov}}[X(k),\tilde Z(k + 1|k)] \cdot {\rm{Va}}{{\rm{r}}^{ - 1}}[\tilde Z(k + 1|k)] \end{array}$$ (26)

      6) 状态预测误差方差阵。

      式(21)给出了状态预测误差方差阵$P(k + 1|k)$的定义,可以得到:

      $$\begin{array}{l} P(k + 1|k) = {\rm{Var}}[\tilde X(k + 1|k)] = \\ {\rm{Var}}[\tilde X(k) + T\delta \tilde X(k + 1|k)] = \\ {T^2}{P_\delta }(k + 1|k) + {\rm{Cov}}[T\delta \tilde X(k + 1|k),\tilde X(k)] + \\ {\rm{Cov}}[\tilde X(k),T\delta \tilde X(k + 1|k)] + P(k) = \\ P(k)(I + TA_\delta ^{\rm{T}}) + {T^2}{P_\delta }(k + 1|k) + T{A_\delta }P(k) \end{array}$$ (27)

      7) 系数$\delta \tilde X(k + 1|k)$和$\hat Z(k + 1|k)$。

      由式(19),易知:

      $$\begin{array}{l} {K_\delta }(k + 1) = {\rm{Cov}}[\delta \tilde X(k + 1),\tilde Z(k + 1|k)] \times \\ {\rm{Va}}{{\rm{r}}^{ - 1}}[\tilde Z(k + 1|k)] \end{array}$$ (28)

      利用$\delta \tilde X(k + 1|k)$和$\hat Z(k + 1|k)$正交以及几何相关知识,可以得出:

      $$\begin{array}{*{20}{l}} {{\rm{Cov}}[\delta \tilde X(k + 1|k),\tilde Z(k + 1|k)] = }\\ {{\rm{Cov}}[\delta \tilde X(k + 1|k),Z(k + 1)] = }\\ {{\rm{Cov}}[\delta \tilde X(k + 1|k),{C_\delta }X(k + 1)] = }\\ {{\rm{Cov}}[\delta \tilde X(k + 1|k),{C_\delta }\tilde X(k + 1|k)] = }\\ {T{P_\delta }(k + 1|k)C_\delta ^{\rm{T}} + {A_\delta }P(k)C_\delta ^{\rm{T}}} \end{array}$$ (29)

      将式(29)带入式(28),整理得到:

      $$\begin{array}{l} {K_\delta }(k + 1) = [T{P_\delta }(k + 1|k) + {A_\delta }P(k)] \times \\ C_\delta ^{\rm{T}}{[{C_\delta }P(k + 1|k)C_\delta ^{\rm{T}} + {R_\delta }(k + 1)]^{ - 1}} \end{array}$$ (30)

      系数${K_d}(k + 1)$由一个协方差和新息方差阵两项组成,新息方差阵由式(21)可以得到,协方差矩阵则可由下式得到:

      $$\begin{array}{l} {\rm{Cov}}[{\bf{X}}(k),{\bf{\tilde Z}}(k + 1|k)] = {\rm{Cov}}[{\bf{\tilde X}}(k),{\bf{\tilde Z}}(k + 1|k)] = \\ {\rm{Cov}}[{\bf{\tilde X}}(k),{{\bf{C}}_\delta }{\bf{\tilde X}}(k + 1|k)] = {\bf{P}}(k)({\bf{I}} + T{\bf{A}}_\delta ^{\rm{T}}){\bf{C}}_\delta ^{\rm{T}} \end{array}$$ (31)

      将式(21)和式(31)带入式(27)中,有:

      $$\begin{array}{l} {{\bf{K}}_d}(k + 1) = {\bf{P}}(k)({\bf{I}} + T{\bf{A}}_\delta ^{\rm{T}}){\bf{C}}_\delta ^{\rm{T}}[{{\bf{C}}_\delta }{\bf{P}}(k + 1|k){\bf{C}}_\delta ^{\rm{T}} + \\ {R_\delta }(k + 1){]^{ - 1}} \end{array}$$ (32)

      8) 状态滤波误差方差阵$P(k + 1)$。

      易知滤波误差为:

      $$\begin{array}{l} {\bf{\tilde X}}(k + 1) = \\ {\bf{\tilde X}}(k) + T\delta {\bf{\tilde X}}(k + 1) - {{\bf{K}}_d}(k + 1){\bf{\tilde Z}}(k + 1|k) \end{array}$$ (33)

      故得滤波误差方差阵为:

      $$\begin{array}{l} P(k + 1) = {\rm{Var}}[\tilde X(k + 1)] = P(k) + {T^2}{P_\delta }(k + 1) + \\ {K_d}(k + 1){\rm{Var}}[\tilde Z(k + 1|k)]K_d^{\rm{T}}(k + 1) + \\ {\rm{Cov}}[\tilde X(k),T\delta \tilde X(k + 1)] + {\rm{Cov}}[T\delta \tilde X(k + 1),\tilde X(k)] - \\ {\rm{Cov}}[\tilde X(k),\tilde Z(k + 1|k)]K_d^{\rm{T}}(k + 1) - \\ {K_d}(k + 1){\rm{Cov}}[\tilde Z(k + 1|k),\tilde X(k)] - \\ {\rm{Cov}}[T\delta \tilde X(k + 1),\tilde Z(k + 1|k)]K_d^{\rm{T}}(k + 1) - \\ {K_d}(k + 1){\rm{Cov}}[\tilde Z(k + 1|k),T\delta \tilde X(k + 1)] \end{array}$$ (34)

      要化简式(30),需要对上式各项进行计算,并注意到:

      $${\rm{Cov}}[T\delta \tilde X(k + 1),\tilde Z(k + 1|k)] = 0$$ (35)
      $${\rm{Cov}}[\tilde Z(k + 1|k),T\delta \tilde X(k + 1)] = 0$$ (36)

      化简可得:

      $$\begin{array}{*{20}{l}} {P(k + 1) = {T^2}{P_\delta }(k + 1) + (I + {A_\delta }T)P(k) + }\\ {P(k)(I + TA_\delta ^{\rm{T}}) - [I + {K_d}(k + 1){C_\delta }(I + {A_\delta }T)]P(k) + }\\ {[I - T{K_\delta }(k + 1){C_\delta }](I + {A_\delta }T)P(k) + }\\ {P(k)(I + TA_\delta ^{\rm{T}})[I - TC_\delta ^{\rm{T}}K_\delta ^{\rm{T}}(k + 1)]} \end{array}$$ (37)

      结合式(24)、式(30)和式(32),可得:

      $$\begin{array}{l} {P_\delta }(k + 1) = {\rm{Var}}[\delta \tilde X(k + 1)] = \\ {P_\delta }(k + 1|k) - {K_\delta }(k + 1){\rm{Var}}[\tilde Z(k + 1|k)]K_\delta ^{\rm{T}}(k + 1) \end{array}$$ (38)

      带入式(37)中,最终得到:

      $$\begin{array}{l} P(k + 1) = {T^2}{P_\delta }(k + 1|k) + \\ [I - T{K_\delta }(k + 1){C_\delta }](I + {A_\delta }T)P(k) + \\ P(k)(I + TA_\delta ^{\rm{T}})[I - TC_\delta ^{\rm{T}}K_\delta ^{\rm{T}}(k + 1)] - \\ [I + {K_d}(k + 1){C_\delta }(I + {A_\delta }T)]P(k) - {T^2}{K_\delta }(k + 1) \times \\ [{C_\delta }P(k + 1|k)C_\delta ^{\rm{T}} + {R_\delta }(k + 1)]K_\delta ^{\rm{T}}(k + 1) \end{array}$$ (39)

      至此得到了基于后向差分Delta算子的卡尔曼滤波递推算法。

    • 在目标跟踪过程中,由于目标的位置、速度、加速度的测量值在任何时候都会产生噪声,卡尔曼滤波能够利用目标的动态信息降低噪声的影响,获得关于目标位置好的估计,因此卡尔曼滤波在目标跟踪有着广泛的应用。

    • 研究目标在平面上沿x轴正方向以速度v=20 m/s作匀速直线运动,状态方程为:

      $$X(k + 1) = FX(k) + GW(k)$$ (40)

      式中,状态向量$X(k) = {[x{\rm{ }}\dot x]^{\rm{T}}}$,状态转移矩阵F、过程噪声分布矩阵G分别为:

      $$F = [1,T;0,1]G = [{T^2}/2;T]$$ (41)

      采样周期为T,过程噪声W(k)为零均值高斯白噪声,方差Q=1。

      量测方程$Z(k) = HX(k) + V(k)$中,量测噪声V(k)是零均值的高斯白噪声,且和过程噪声序列相互独立,方差R=4,量测矩阵H=[1 0]。

    • 假设目标起始点位置坐标为$(5\;{\rm{m , }}0\;{\rm{m}})$,初始协方差矩阵$P = [R,R/T;R/T,2R/{T^2}]$,在采样周期$T = 1\;{\rm{s}}$时,目标真实运动轨迹和常规卡尔曼滤波后轨迹以及Delta滤波后轨迹如图 1所示,滤波后位置误差曲线如图 2所示。

      图  1  真实和滤波轨迹图($T = 1\;{\rm{s}}$)

      图  2  滤波后位置误差图($T = 1\;{\rm{s}}$)

      图 1直观地反映了通过常规卡尔曼滤波后的轨迹与基于后向差分Delta算子的递推卡尔曼滤波后的轨迹与真实轨迹的对比,可以看出两者滤波效果差异不明显。而图 2不仅反映了两种滤波方法的收敛速度相当,另一方面从位置误差的波动幅度反映出两者滤波性能亦相当。

      在采样周期$T = 0.01\;{\rm{s}}$时,通过常规卡尔曼滤波以及基于后向差分Delta算子的递推卡尔曼滤波后,跟踪目标的位置误差图对比曲线如图 3所示。

      图  3  滤波后位置误差图($T = 0.1\;{\rm{s}}$)

      在采样周期$T = 1\;{\rm{s}}$时,两种滤波器滤波后跟踪目标的位置误差对比曲线如图 4所示。

      图  4  滤波后位置误差图($T = 0.01\;{\rm{s}}$)

      由仿真结果可以看到,当$T = 1\;{\rm{s}}$时,基于后向差分Delta算子的递推卡尔曼滤波算法和常规卡尔曼滤波算法在收敛速度上相当,滤波器性能前者较之后者没有明显优势。但当$T = 0.1\;{\rm{s}}$和$T = 0.01\;{\rm{s}}$时,由滤波后位置误差图 3图 4所呈现的误差收敛曲线反映出基于后向差分Delta算子的递推卡尔曼滤波算法的收敛性优于常规卡尔曼滤波算法,并且随着采样周期T的减少,基于后向差分Delta算子的递推卡尔曼在收敛速度以及误差波动幅度上均明显优于常规卡尔曼滤波。

    • 本文基于Delta算子在高频采样情形下使离散化系统模型趋近于连续系统模型的特性,结合正交投影法推导了基于Delta算子的递推卡尔曼滤波算法,包含Delta状态滤波方程、新息方差阵、Delta状态最优预测、Delta状态预测方差阵的计算、状态滤波方程的预测、状态预测误差方差阵的计算和状态滤波误差方差阵,并进行了仿真实验。由仿真结果可以得到,随着采样周期T减小,常规卡尔曼滤波算法收敛性能变慢,推导的基于后向差分Delta算子的递推卡尔曼滤波算法滤波性能良好,在高速采样情形下,其在速度收敛性以及误差波动幅度方面均优于常规卡尔曼滤波算法。

参考文献 (12)

目录

    /

    返回文章
    返回