α-β滤波器(一种1维稳态Kalman滤波器)详解

α − β \alpha-\beta αβ滤波器

文章目录

  • α − β \alpha-\beta αβ滤波器
    • 状态方程和观测方程
    • α − β \alpha-\beta αβ滤波器最终形式
    • α − β \alpha-\beta αβ滤波器作为一种稳态Kalman滤波器
      • 状态方程、量测方程
      • 推导增益 K K K
      • 结论
    • α , β \alpha,\beta α,β参数的工程化设置
    • 仿真
      • 工况1 初始估计误差很大
      • 工况2 观测噪声很大
      • 工况3 恒定加速度扰动
      • 工况4 增大a、b的结果
    • 总结
    • Ref

本博客来自一个课程的作业,包含以下内容:

  • 一.α-β滤波器推导
  • 二.作为稳态滤波器与Kalman滤波的比较
  • 三.α-β的参数设置及其最优性
  • 四.仿真演示
  • 五.总结

KF是噪声干扰下、线性系统最优的滤波器,而a-b滤波器是KF的衍生物,在性质、结构上都很类似,但在计算复杂性上大大降低。KF动态地调整系数,以最大效率的滤除观测误差,使对应的状态估计值最佳。而a-b滤波则使用固定的滤波增益,或者认为给定好的变化增益。所以a-b滤波器是次优的。通过调整系数,用户可以控制用于从最近的测量中历史数据的长度,并用来估计数据的真实值。

对一标量 x ( k ) x(k) x(k),存在对它的一系列观测值 { z ( k ) } \{z(k)\} {z(k)},则 α − β \alpha-\beta αβ滤波器的预测和滤波方程为
x ( k + 1 ∣ k ) = x ^ ( k ) + Δ t ⋅ v ^ ( k ) x ^ ( k + 1 ) = x ( k + 1 ∣ k ) + α ⋅ [ z ( k + 1 ) − x ( k + 1 ∣ k ) ] v ^ ( k + 1 ) = v ^ ( k ) + β / Δ t ⋅ [ z ( k + 1 ) − x ( k + 1 ∣ k ) \begin{aligned} x(k+1|k)&=\hat x(k) + \Delta t\cdot\hat v(k)\\ \hat x(k+1)&=x(k+1|k)+\alpha \cdot[z(k+1)-x(k+1|k)]\\ \hat v(k+1)&=\hat v(k)+\beta/\Delta t \cdot [z(k+1)-x(k+1|k) \end{aligned} x(k+1k)x^(k+1)v^(k+1)=x^(k)+Δtv^(k)=x(k+1k)+α[z(k+1)x(k+1k)]=v^(k)+β/Δt[z(k+1)x(k+1k)其中 Δ t \Delta t Δt为观测的更新时间。参数 α , β \alpha,\beta α,β的设置依赖于对数据机动量噪声估计的大小、观测误差的大小。

状态方程和观测方程

a-b滤波和a-b-g滤波都要解决这样一个问题:对1维数据 x ( k ) x(k) x(k),已知这个数据的一系列观测值 { x ( 0 ) , x ( 1 ) , ⋯   , x ( k ) } \{x(0),x(1),\cdots,x(k)\} {x(0),x(1),,x(k)},然后要利用已有数据进行滤波估计,得到更准确和平滑的值。

通常使用场景:假设1维场景的目标以匀速直线运动,而能够测量到距离序列,需要估计它的真实距离。

a-b滤波器不要求已知数学模型,它所考虑的只有状态在时域的连续性,也就是状态量 x x x的一阶导和二阶导存在如下关系
x ˙ = v v ˙ = a \dot x=v \\ \dot v=a x˙=vv˙=a这个式子如果改成离散型,一阶或二阶形式分别就是a-b滤波,a-b-g滤波的系统方程
x ( k + 1 ) = x ( k ) + Δ t ⋅ v ( k ) x ( k + 1 ) = x ( k ) + Δ t ⋅ v ( k ) + Δ t 2 2 ⋅ a ( k ) x(k+1)=x(k)+\Delta t\cdot v(k) \\ x(k+1)=x(k)+\Delta t\cdot v(k)+ \frac{\Delta t^2}{2}\cdot a(k) x(k+1)=x(k)+Δtv(k)x(k+1)=x(k)+Δtv(k)+2Δt2a(k)改写成state-space的离散形式:
x ( k + 1 ∣ k ) = Φ ( k + 1 ∣ k ) x ( k ) 2个状态:位置-速度: Φ ( k + 1 ∣ k ) = [ 1 Δ t 0 1 ] , x = [ x v ] 3个状态:位置-速度-加速度: Φ ( k + 1 ∣ k ) = [ 1 Δ t Δ t 2 2 0 1 Δ t 0 0 1 ] , x = [ x v a ] {\boldsymbol x}(k+1|k)=\boldsymbol \Phi(k+1|k){\boldsymbol x}(k)\\ \text{2个状态:位置-速度:} \boldsymbol \Phi(k+1|k)=\begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} , \boldsymbol{x}=\begin{bmatrix} x \\ v \end{bmatrix}\\ \text{3个状态:位置-速度-加速度:} \boldsymbol \Phi(k+1|k)=\begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2}\\ 0 & 1 & \Delta t\\ 0 & 0& 1 \end{bmatrix} , \boldsymbol{x}=\begin{bmatrix} x \\ v \\ a \end{bmatrix} x(k+1k)=Φ(k+1k)x(k)2个状态:位置-速度:Φ(k+1k)=[10Δt1],x=[xv]3个状态:位置-速度-加速度:Φ(k+1k)=100Δt102Δt2Δt1,x=xva而观测方程往往是只有位置,只有1维,对a-b滤波而言
z ( k ) = [ 1 0 ] [ x ( k ) v ( k ) ] z(k)= \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} x(k)\\ v(k) \end{bmatrix} z(k)=[10][x(k)v(k)]对a-b-g滤波而言,
z ( k ) = [ 1 0 0 ] [ x ( k ) v ( k ) a ( k ) ] z(k)= \begin{bmatrix} 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} x(k)\\ v(k)\\ a(k) \end{bmatrix} z(k)=[100]x(k)v(k)a(k)

如果你的实际模型和这个不一样,很显然,它是被一个2阶Markov过程近似了,而其他的内部关系都没有考虑,都被当做了噪声。也就是说,噪声项 w ( k ) w(k) w(k)直接施加在加速度上,以3阶为例:
x ( k + 1 ) = [ 1 Δ t Δ t 2 2 0 1 Δ t 0 0 1 ] [ x ( k ) v ( k ) a ( k ) ] + [ Δ t 2 2 Δ t 1 ] w ( k ) \boldsymbol{x}(k+1)= \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2}\\ 0 & 1 & \Delta t\\ 0 & 0& 1 \end{bmatrix} \begin{bmatrix} x(k) \\ v(k) \\ a(k) \end{bmatrix} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \\ 1 \end{bmatrix}w(k) x(k+1)=100Δt102Δt2Δt1x(k)v(k)a(k)+2Δt2Δt1w(k)

z ( k ) = [ 1 0 0 ] [ x ( k ) v ( k ) a ( k ) ] + n ( k ) z(k)= \begin{bmatrix} 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} x(k)\\ v(k)\\ a(k) \end{bmatrix} + n(k) z(k)=[100]x(k)v(k)a(k)+n(k)其中 w ( k ) , n ( k ) w(k),n(k) w(k),n(k)均是不相关的高斯白噪声,互相独立,设他们的方差为 Q = σ w 2 , R = σ n 2 Q=\sigma_w^2,R=\sigma_n^2 Q=σw2,R=σn2. 3阶状态方程的 Γ = [ Δ t 2 / 2 , Δ t , 1 ] T \boldsymbol \Gamma =[\Delta t^2/2, \Delta t, 1]^{\mathrm T} Γ=[Δt2/2,Δt,1]T,2阶方程的噪声增益为 Γ = [ Δ t 2 / 2 , Δ t ] T \boldsymbol \Gamma =[\Delta t^2/2, \Delta t]^{\mathrm T} Γ=[Δt2/2,Δt]T

α − β \alpha-\beta αβ滤波器最终形式

需要从已有的位置观测序列 { z ( 0 ) , z ( 1 ) , ⋯   , z ( k ) , z ( k + 1 ) } \{z(0),z(1),\cdots,z(k),z(k+1)\} {z(0),z(1),,z(k),z(k+1)}中估计位置 x ^ ( k + 1 ) \hat x(k+1) x^(k+1)、速度 v ^ ( k + 1 ) \hat v(k+1) v^(k+1)。类似地a-b-g滤波器需要从已有的位置观测序列 { z ( 0 ) , z ( 1 ) , ⋯   , z ( k ) , z ( k + 1 } \{z(0),z(1),\cdots,z(k),z(k+1\} {z(0),z(1),,z(k),z(k+1}中估计位置 x ^ ( k + 1 ) \hat x(k+1) x^(k+1)、速度 v ^ ( k + 1 ) \hat v(k+1) v^(k+1)、加速度 a ^ ( k + 1 ) \hat a(k+1) a^(k+1),实际上由于种种原因,估计出来的导数项如速度、加速度都是不可用的,但滤波方程中仍然包含这些项,以保证连续性。

类似于KF的滤波过程,分为预测更新2部分。利用上一轮的滤波数据,其中包含在新一轮观测更新时,新的预测为
x ( k + 1 ∣ k ) = x ^ ( k ) + Δ t ⋅ v ^ ( k ) v ( k + 1 ∣ k ) = v ^ ( k ) x(k+1|k)=\hat x(k) + \Delta t\cdot\hat v(k)\\ v(k+1|k)=\hat v(k) x(k+1k)=x^(k)+Δtv^(k)v(k+1k)=v^(k)新的测量为 z ( k + 1 ) z(k+1) z(k+1),则观测误差为
z ~ ( k + 1 ) = z ( k + 1 ) − x ( k + 1 ∣ k ) \tilde z(k+1)=z(k+1)-x(k+1|k) z~(k+1)=z(k+1)x(k+1k)

滤波方程使用残差的 α 倍来校正位置估计,使用残差的 β/T 倍来校正速度估计,即最终结果为
x ^ ( k + 1 ) = x ( k + 1 ∣ k ) + α ⋅ z ~ ( k + 1 ) v ^ ( k + 1 ) = v ( k + 1 ∣ k ) + β / Δ t ⋅ z ~ ( k + 1 ) \hat x(k+1)=x(k+1|k)+\alpha \cdot\tilde z(k+1)\\ \hat v(k+1)=v(k+1|k)+\beta/\Delta t \cdot \tilde z(k+1) x^(k+1)=x(k+1k)+αz~(k+1)v^(k+1)=v(k+1k)+β/Δtz~(k+1)其中第二个式子也通常把 Δ t \Delta t Δt直接引进去。a-b滤波的参数 α , β \alpha,\beta α,β需要实际调整。

α − β \alpha-\beta αβ滤波器作为一种稳态Kalman滤波器

状态方程、量测方程

  • 预测状态 x ^ ( k + 1 ∣ k ) = Φ ( k + 1 ∣ k ) x ^ ( k ) \hat{\boldsymbol x}(k+1|k)=\boldsymbol \Phi(k+1|k)\hat{\boldsymbol x}(k) x^(k+1k)=Φ(k+1k)x^(k)
  • 修正 x ^ ( k + 1 ∣ k + 1 ) = x ^ ( k + 1 ∣ k ) + K [ z ( k + 1 ) − H x ^ ( k + 1 ∣ k ) ] \hat{\boldsymbol x}(k+1|k+1)=\hat{\boldsymbol x}(k+1|k)+\boldsymbol K[z(k+1)-\boldsymbol H\hat{\boldsymbol x}(k+1|k)] x^(k+1k+1)=x^(k+1k)+K[z(k+1)Hx^(k+1k)]其中观测矩阵 h = [ 1 , 0 ] \boldsymbol h=[1,0] h=[1,0],Kalman增益是一个定常矩阵
    K = [ α β / Δ t ] \boldsymbol K=\begin{bmatrix} \alpha \\ \beta/\Delta t \end{bmatrix} K=[αβ/Δt]这2个方程与Kalman滤波完全一样。

推导增益 K K K

根据Kalman滤波协方差矩阵稳态的条件,可以推出其余的3个方程:

  • 状态协方差预测

P ( k + 1 ∣ k ) = Φ P ( k ∣ k ) Φ T + Γ σ w 2 Γ T \boldsymbol P(k+1|k)=\boldsymbol\Phi \boldsymbol P(k|k) \boldsymbol\Phi^{\mathrm T}+ \boldsymbol\Gamma \sigma^2_w \boldsymbol\Gamma^{\mathrm T} P(k+1k)=ΦP(kk)ΦT+Γσw2ΓT

  • 目标跟踪增益(方括号里的矩阵求逆退化为1维标量求倒数)

K ( k + 1 ) = P ( k + 1 ∣ k ) h T [ h P ( k + 1 ∣ k ) h T + σ n 2 ] − 1 \boldsymbol K(k+1) =\boldsymbol P(k+1|k)\boldsymbol h^{\mathrm T} [\boldsymbol h \boldsymbol P(k+1|k)\boldsymbol h^{\mathrm T}+\sigma_n^2]^{-1} K(k+1)=P(k+1k)hT[hP(k+1k)hT+σn2]1

  • 协方差更新

P ( k + 1 ∣ k + 1 ) = [ I 2 − K ( k + 1 ) h ] P ( k + 1 ∣ k ) \boldsymbol P(k+1|k+1)= [I_2-\boldsymbol K(k+1)\boldsymbol h] \boldsymbol P(k+1|k) P(k+1k+1)=[I2K(k+1)h]P(k+1k)
构造这样的关系,即预测出的协方差矩阵已经趋于稳态:
P ( k + 1 ∣ k ) = P ( k ∣ k − 1 ) = P \boldsymbol P(k+1|k)=\boldsymbol P(k|k-1)=\boldsymbol P P(k+1k)=P(kk1)=P将稳态形式的Kalman滤波的三个方程代入,得到Riccati方程形式
Φ [ P − P H ⊤ ( H P H ⊤ + R ) − 1 H P ] Φ ⊤ + Γ Q Γ ⊤ = P ↓ Φ P Φ ⊤ − Φ P H ⊤ ( H P H T + σ n 2 ) − 1 H P Φ ⊤ + Γ σ w 2 Γ ⊤ = P \Phi\left[P-P H^{\top}\left(H P H^{\top}+R\right)^{-1} H P\right] \Phi^{\top}+\Gamma Q \Gamma^{\top}=P\\ \downarrow\\ \Phi P \Phi^{\top}-\Phi P H^{\top}\left( H P H^{\mathrm T}+\sigma_n^2\right)^{-1} H P \Phi^{\top}+\Gamma \sigma_w^2 \Gamma^{\top}=P Φ[PPH(HPH+R)1HP]Φ+ΓQΓ=PΦPΦΦPH(HPHT+σn2)1HPΦ+Γσw2Γ=P求解预测的协方差矩阵
P = [ P 11 P 12 P 21 P 22 ] \boldsymbol P=\begin{bmatrix} P_{11} & P_{12} \\ P_{21} & P_{22} \end{bmatrix} P=[P11P21P12P22]通过将所有的项代入,可以在mathematica中解出3个方程的解,共有4组可行解,再考虑到协方差矩阵的正定性,只剩下一组可行解。
α-β滤波器(一种1维稳态Kalman滤波器)详解_第1张图片α-β滤波器(一种1维稳态Kalman滤波器)详解_第2张图片

即作为其他几个变量的函数
P 11 ↦ ( Δ t , σ w , σ n ) P 12 ↦ ( Δ t , σ w , σ n ) P 22 ↦ ( Δ t , σ w , σ n ) P_{11}\mapsto(\Delta t,\sigma_w,\sigma_n)\\ P_{12}\mapsto(\Delta t,\sigma_w,\sigma_n)\\ P_{22}\mapsto(\Delta t,\sigma_w,\sigma_n) P11(Δt,σw,σn)P12(Δt,σw,σn)P22(Δt,σw,σn)

将这个表达式代入,就可以得到滤波增益:
K = [ α β / Δ t ] = 1 σ n 2 + P 11 [ P 11 P 21 ] \boldsymbol K=\begin{bmatrix} \alpha \\ \beta/\Delta t \end{bmatrix} =\frac{1}{\sigma_n^2+P_{11}} \begin{bmatrix} P_{11}\\ P_{21} \end{bmatrix} K=[αβ/Δt]=σn2+P111[P11P21]这样,就可以构造关系:
Δ t , σ w , σ n ↦ P 11 , P 12 ↦ α , β \Delta t,\sigma_w,\sigma_n \mapsto P_{11},P_{12} \mapsto \alpha,\beta Δt,σw,σnP11,P12α,β但是,由于前一个表达式过于复杂,不可能把这么长的表达式代入。因此求解结果仍然没有应用价值。

结论

结论:a-b滤波器是Kalman滤波器作为稳态滤波器的衍生物,在性质、结构上与KF都很类似。

  • KF有明确的状态空间模型,而a-b滤波器将状态简化为二阶噪声驱动的Markov随机过程
  • KF有多维观测量,而a-b滤波器只有所有状态的第1维。但两者都是将观测直接加白噪声。
  • KF预测协方差矩阵,而a-b滤波器只关注状态的连续性,不考虑不确定部分
  • KF是噪声干扰下、线性系统最优的滤波器,而a-b滤波器是稳态的固定系数的,次优的滤波器

α , β \alpha,\beta α,β参数的工程化设置

KF动态地调整系数,以最大效率的滤除观测误差,使对应的状态估计值最佳。而a-b滤波则使用固定的滤波增益,或者认为给定好的变化增益。

文献给出了一种根据物理意义明确的设置方法,定义了跟踪指数tracking index
Λ ≜ Δ t 2 ⋅ σ w σ n \Lambda\triangleq \frac{\Delta t^2\cdot\sigma_w}{\sigma_n} ΛσnΔt2σw其实就是位置的机动(加速度上的噪声 w ( k ) w(k) w(k))误差除以测量误差(测量方程噪声 n ( k ) n(k) n(k)),作为模型误差与观测误差的比值,根据它,可以直接约束 α \alpha α,进而确定 β \beta β

  • 如果观测不可靠,则跟踪指数小,会更倾向于已有的数据;
  • 如果目标机动量大,则跟踪指数大,则更倾向于观测的数据。
  • 跟踪指数小,滤波结果更平滑,但收敛更慢。但是不至于导致发散。

按照它,最优的参数准则应该满足以下关系:
β 2 1 − α = Λ 2 \frac{\beta^{2}}{1-\alpha}=\Lambda^{2} 1αβ2=Λ2经过在Kalman滤波器框架下的推导,证明等价于最小方差的最优稳态滤波器,实际上就是Riccati方程的化简解。化简解还有
r = 4 + Λ − 8 Λ + Λ 2 4   ,   α = 1 − r 2 β = 2 ( 2 − α ) − 4 1 − α r=\frac{4+\Lambda-\sqrt{8 \Lambda+\Lambda^{2}}}{4}\ ,\ \alpha=1-r^{2}\\ \beta=2(2-\alpha)-4 \sqrt{1-\alpha} r=44+Λ8Λ+Λ2  , α=1r2β=2(2α)41α 根据跟踪指数可以获得最优的 α \alpha α,进而得到最优的 β \beta β,两个参数之间的关系如下图所示。
α-β滤波器(一种1维稳态Kalman滤波器)详解_第3张图片
α-β滤波器(一种1维稳态Kalman滤波器)详解_第4张图片

此外,Kalata还给出了其他的一些类型如 α \alpha α滤波器, α − β − γ \alpha-\beta-\gamma αβγ滤波器的参数设置。

α-β滤波器(一种1维稳态Kalman滤波器)详解_第5张图片

由于根号内部不能小于0,所以还有一些不等式条件:
0 < α < 1 0 < β ≤ 2 0 < 4 − 2 α − β \begin{aligned} &0<\alpha<1 \\ &0<\beta \leq 2 \\ &0<4-2 \alpha-\beta \end{aligned} 0<α<10<β20<42αβ

Kalata还给出了滤波过程的初始化,以及性能指标的预估。虽然这几个滤波器都是全局收敛的。:

α-β滤波器(一种1维稳态Kalman滤波器)详解_第6张图片

仿真

搭建一个1维模拟的模块,模拟匀速直线运动,加速度项用噪声模拟。滤波器根据观测噪声的比值,用最优的参数配置。

观测更新率 d T dT dT 10Hz
观测误差(高斯白噪声,1 σ \sigma σ σ n \sigma_n σn 1m
目标实际运动
机动性模拟(高斯白噪声,1 σ \sigma σ σ w \sigma_w σw 0.03m/s2
加速度 a ( 0 ) a(0) a(0) 0
速度 v ( 0 ) v(0) v(0) 10m/s
初始距离 x ( 0 ) x(0) x(0) 980m
滤波器
初始距离估计 x ^ ( 0 ) \hat x(0) x^(0) 1200m
初始位置估计 v ^ ( 0 ) \hat v(0) v^(0) 5m/s
加速度估计
跟踪指数 Δ t 2 ⋅ σ w σ n \frac{\Delta t^2\cdot\sigma_w}{\sigma_n} σnΔt2σw 3e-4
α \alpha α 0.0242
β \beta β 0.0003

α-β滤波器(一种1维稳态Kalman滤波器)详解_第7张图片

工况1 初始估计误差很大

目标机动量较小,基本沿匀速直线运动,观测噪声一般,但初始估值较差。结果如下

α-β滤波器(一种1维稳态Kalman滤波器)详解_第8张图片

估计太差,而观测器显然不会出现这样的问题,所以此时收敛非常慢。要收敛正常,可增大增益a、b,或者按照跟踪指数的原则,增大 Λ \Lambda Λ,得到

α \alpha α 0.1362 β \beta β 0.0099

此时,收敛更快
α-β滤波器(一种1维稳态Kalman滤波器)详解_第9张图片

工况2 观测噪声很大

目标偏离直线运动较多 σ w = 1 m / s 2 \sigma_w=1m/s^2 σw=1m/s2,,观测噪声很大 σ n = 3.16 m \sigma_n=3.16m σn=3.16m,初始估值较差,此时滤波结果仍为平滑的

α-β滤波器(一种1维稳态Kalman滤波器)详解_第10张图片

工况3 恒定加速度扰动

目标短暂地加速,加速度 a = 10 m / s 2 a=10m/s^2 a=10m/s2,机动量噪声较大 σ w = 1 m / s 2 \sigma_w=1m/s^2 σw=1m/s2,观测噪声一般 σ n = 1 m \sigma_n=1m σn=1m,初始估值较差,此时

α-β滤波器(一种1维稳态Kalman滤波器)详解_第11张图片α-β滤波器(一种1维稳态Kalman滤波器)详解_第12张图片

工况4 增大a、b的结果

α \alpha α 0.1362 β \beta β 0.0099
α-β滤波器(一种1维稳态Kalman滤波器)详解_第13张图片

α \alpha α 0.75 β \beta β 0.5

α-β滤波器(一种1维稳态Kalman滤波器)详解_第14张图片

小的参数,更平滑,但会在目标动态机动时产生滞后误差;大的参数,跟踪性能更好,但滤波曲线波动更多。

总结

  • 对高斯白噪声影响下的动态数据,未知系统模型,根据现有的数据观测,滤波效率非常高、收敛性也较好。
  • 只能处理1维的观测数据,适用于计算量要求苛刻的简单问题。
  • 滤波性能十分依赖于α、β 参数,越大,代表更相信观测数据,滤波的噪声越多;越小,代表更相信历史数据,也就是收敛更慢。
  • 作为稳态卡尔曼滤波的简化形式,存在一个最优的设置,它依赖于观测更新时间、机动量噪声、观测噪声

Ref

  1. Mathworks-alphabetafilter
  2. Robert Penoyer - The Alpha-Beta Filter
  3. Wikipedia - Alpha_beta filter
  4. Kalata, P. R. (1984). The Tracking Index: A Generalized Parameter for α-β and α-β-γ Target Trackers. IEEE Transactions on Aerospace and Electronic Systems, AES-20(2), 174-182. doi:10.1109/TAES.1984.310438

你可能感兴趣的:(导航定位,算法)