自动驾驶(十三)---------无损卡尔曼滤波

       卡尔曼滤波核心原理是根据系统的估算及协方差、测量值及协方差推算出最优的估计值及协方差。在非线性模型中,扩展卡尔曼滤波丢弃了高阶项,当状态向量复杂时候,求雅克比矩阵难度也会上升。而UKF类似于将高斯分布中的均值,方差通过非线性方程处理后,去匹配预测方程的非线性变换。

       假设初始状态满足高斯分布,如X ∼ N(µ, σ^2 ),如经过非线性状态转移方程以后,如何求变换过后的期望和方差呢?

       UKF认为:每一个状态x_k,P_k(期望和方差),都可以用几个关键点X_{sig}(Sigma点)来表示,当作用于非线性函数f(x)时,只需要将Sigma点作用于非线性函数f(x)得到即可,通过新得到的(关键点)可以计算新的分布状态

       通过上面的介绍,出现几个问题:

  1. 关键点怎么找
  2. 找到关键点后如何求出新的状态xk+1,Pk+1

1.关键点怎么找

      关键点的意义在于能够充分刻画原状态的分布情况:

                                                                   

       其中为当前分布的均值,\lambda为尺度参数,一般采用经验值λ=3−nx,调整其值可以提高逼近精度,n为变量\mathbf x的维度。用这一组采样点可以近似地表示变量\mathbf x的高斯分布。

2.求新的状态xk+1,Pk+1

       通过上面步骤选取了对应的点,又因为已知状态转移公式(这个必须知道,例如车辆运动学公式),因为Kalman只适用于线性状态转移,而一般模型都是非线性的,想使用Kalman必须转化成线性,UKF就是要解决这个问题。

       通过状态转移得到新的点,下面计算新的状态:

  1. 计算函数映射后的均值和方差

              均值计算方式:  \mathbf{\bar{y}}\approx \sum\limits_{i=1}^{2n}{W_{i}^{(m)}}{​{\mathbf{Y}}_{i}},其中Y_i 是新计算出的点,为每个点的权重。

              方差计算方式:\bar{y}是新点集的均值,为每个点的权重。

    2. 计算权重W_{i}^{(m)}W_{i}^{(c)}

              W_{i}^{(m)}:在i==0时: W_{0}^{(m)}=\frac{\lambda }{n+\lambda },   在i!=0时:W_{i}^{(m)}=  

              W_{i}^{(c)}:在i==0时: W_{0}^{(c)}=\frac{\lambda }{n+\lambda }+\left( 1-{​{\alpha }^{2}}+\beta \right),   在i!=0时:  W_{i}^{(c)}=  

         其中\lambda 第一步中使用过的尺度参数,\alpha的取值一般在[1e^{-4},1)区间内;\beta为状态分布参数,对高斯分布的变量,\beta =2最优,如果状态变量是标量的话,则\beta =0最优。

          

  • 对非线性函数的概率密度分布进行近似,而不是对非线性函数做近似,算法实现不受模型复杂度的影响;
  • 所得到的的非线性函数的统计量的准确性可以达到三阶,而之前EKF的准确性只能达到一阶;
  • 无需计算Jacobi矩阵,这样就能处理不可导的非线性函数。

上面讲到UKF如何解决最关键的协方差矩阵的传递,下面介绍UKF的全过程

简化UKF滤波算法的流程为:

(1)初始化

             \left\langle {​{\mathbf{\theta }}_{0}} \right\rangle =E\left[ {​{\mathbf{\theta }}_{0}} \right]

              {​{\mathbf{\Sigma }}_{\mathbf{\theta },0}}=E\left[ \left( {​{\mathbf{\theta }}_{0}}-\left\langle {​{\mathbf{\theta }}_{0}} \right\rangle \right){​{\left( {​{\mathbf{\theta }}_{0}}-\left\langle {​{\mathbf{\theta }}_{0}} \right\rangle \right)}^{T}} \right]

(2)状态估计

         ①计算Sigma点

                   \mathbf{\chi }_{k-1}^{0}=\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle ,i=0

                   \mathbf{\chi }_{k-1}^{i}=\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle +{​{\left( \sqrt{(n+\lambda )\cdot {​{\Sigma }_{\theta ,k-1}}} \right)}_{i}},i=1,...,n

                   \mathbf{\chi }_{k-1}^{i}=\left\langle {​{\mathbf{\theta }}_{k-1}} \right\rangle -{​{\left( \sqrt{(n+\lambda )\cdot {​{\Sigma }_{\theta ,k-1}}} \right)}_{i}},i=n+1,...,2n

         ②时间传播方程

                   \mathbf{\chi }_{k|k-1}^{i}=f\left( \mathbf{\chi }_{k-1}^{i} \right)

                   \theta _{k}^{'}=\sum\limits_{i=0}^{2n}{W_{i}^{(m)}\mathbf{\chi }_{k|k-1}^{i}}

                   \mathbf{\Sigma }_{\mathbf{\theta },k}^{'}=\sum\limits_{i=0}^{2n}{W_{i}^{(c)}\left( \mathbf{\chi }_{k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right){​{\left( \mathbf{\chi }_{k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right)}^{T}}}+{​{\mathbf{Q}}}

                    \mathbf{\gamma }_{k|k-1}^{i}=h\left( \mathbf{\chi }_{k|k-1}^{i} \right)

                    \mathbf{z}_{k}^{'}=\sum\limits_{i=0}^{2n}{W_{i}^{(m)}\mathbf{\gamma }_{k|k-1}^{i}}

          ③状态更新方程

                    {​{\mathbf{\Sigma }}_{\mathbf{z},k}}=\sum\limits_{i=0}^{2n}{W_{i}^{(c)}\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right){​{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}+{​{\mathbf{R}}}

                    {​{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}=\sum\limits_{i=0}^{2n}{W_{i}^{(c)}\left( \mathbf{\chi }_{k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right){​{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}

                    {​{\mathbf{K}}_{k}}={​{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}\cdot \mathbf{\Sigma }_{\mathbf{z},k}^{-1}

                     \left\langle {​{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{​{\mathbf{K}}_{k}}({​{\mathbf{z}}_{k}}-\mathbf{z}_{k}^{'})

                    {​{\mathbf{\Sigma }}_{\mathbf{\theta },k}}=\mathbf{\Sigma }_{\mathbf{\theta },k}^{'}-{​{\mathbf{K}}_{k}}{​{\mathbf{\Sigma }}_{\mathbf{z},k}}\mathbf{K}_{k}^{T}

        以上就是简化UKF滤波算法的全部过程。

 

扩维UKF滤波算法(噪声隐含)

          系统的状态转移方程和观测方程为

                        {​{\mathbf{\theta }}_{k}}=f({​{\mathbf{\theta }}_{k-1}},{​{\mathbf{s}}_{k}}),      \small {​{\mathbf{s}}_{k}}\sim \mathcal{N}(0,\mathbf{Q})

                        {​{\mathbf{z}}_{k}}=h({​{\mathbf{\theta }}_{k}},{​{\mathbf{v}}_{k}}),      \small {​{\mathbf{v}}_{k}}\sim \mathcal{N}(0,\mathbf{R})

          此时需要对状态变量进行扩展,得到增广状态向量

                       {​{\mathbf{\theta }}_{a,k}}=\left[ {​{\mathbf{\theta }}_{k}};{​{\mathbf{s}}_{k}};{​{\mathbf{v}}_{k}} \right]

          增广状态的均值和协方差矩阵分别为

                     \left\langle {​{\mathbf{\theta }}_{a,k}} \right\rangle =\left[ \left\langle {​{\mathbf{\theta }}_{k}} \right\rangle ;{​{\mathbf{0}}_{m\times 1}};{​{\mathbf{0}}_{l\times 1}} \right]

                         {​{\mathbf{\Sigma }}_{\mathbf{a},k}}=\left[ \begin{matrix} {​{\mathbf{\Sigma }}_{\mathbf{\theta },k}} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{Q} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{R} \\ \end{matrix} \right]

           其中ml分别表示状态噪声和观测噪声的维度。

       扩维UKF滤波算法的流程为:

    (1)初始化:

                     \left\langle {​{\mathbf{\theta }}_{0}} \right\rangle =E\left[ {​{\mathbf{\theta }}_{0}} \right]

                     {​{\mathbf{\Sigma }}_{\mathbf{\theta },0}}=E\left[ \left( {​{\mathbf{\theta }}_{0}}-\left\langle {​{\mathbf{\theta }}_{0}} \right\rangle \right){​{\left( {​{\mathbf{\theta }}_{0}}-\left\langle {​{\mathbf{\theta }}_{0}} \right\rangle \right)}^{T}} \right]

                     \left\langle {​{\mathbf{\theta }}_{\mathbf{a},0}} \right\rangle =E\left[ \left\langle {​{\mathbf{\theta }}_{0}} \right\rangle ;{​{\mathbf{0}}_{m\times 1}};{​{\mathbf{0}}_{l\times 1}} \right]

                     {​{\mathbf{\Sigma }}_{\mathbf{a},0}}=\left[ \begin{matrix} {​{\mathbf{\Sigma }}_{\mathbf{\theta },0}} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{Q} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{R} \\ \end{matrix} \right]

    (2)状态估计

               ①计算Sigma点

                      \mathbf{\chi }_{a,k-1}^{0}=\left\langle {​{\mathbf{\theta }}_{a,k-1}} \right\rangle ,i=0

                      \mathbf{\chi }_{a,k-1}^{i}=\left\langle {​{\mathbf{\theta }}_{a,k-1}} \right\rangle +{​{\left( \sqrt{(n+\lambda )\cdot {​{\Sigma }_{a,k-1}}} \right)}_{i}},i=1,...,N

                      \mathbf{\chi }_{a,k-1}^{i}=\left\langle {​{\mathbf{\theta }}_{a,k-1}} \right\rangle -{​{\left( \sqrt{(n+\lambda )\cdot {​{\Sigma }_{a,k-1}}} \right)}_{i}},i=N+1,...,2N

                       N=n+m+l表示增广状态的维度。

                       {​{\mathbf{\chi }}_{\mathbf{a},k}}={​{\left[ \mathbf{\chi }_{\mathbf{\theta },k}^{T},\mathbf{\chi }_{\mathbf{s},k}^{T},\mathbf{\chi }_{\mathbf{v},k}^{T} \right]}^{T}}

                ②时间传播方程

                       \mathbf{\chi }_{\mathbf{\theta },k|k-1}^{i}=f\left( \mathbf{\chi }_{\mathbf{\theta },k-1}^{i},\mathbf{\chi }_{\mathbf{s},k-1}^{i} \right)

                         \theta _{k}^{'}=\sum\limits_{i=0}^{2N}{W_{i}^{(m)}\mathbf{\chi }_{​{\theta },k|k-1}^{i}}

                       \mathbf{\Sigma }_{\mathbf{\theta },k}^{'}=\sum\limits_{i=0}^{2N}{W_{i}^{(c)}\left( \mathbf{\chi }_{\mathbf{\theta },k|k-1}^{i}-\mathbf{\theta }_{\mathbf{a},k}^{'} \right){​{\left( \mathbf{\chi }_{\mathbf{a},k|k-1}^{i}-\mathbf{\theta }_{\mathbf{a},k}^{'} \right)}^{T}}}

                        \mathbf{\gamma }_{k|k-1}^{i}=h\left( \mathbf{\chi }_{\mathbf{\theta },k|k-1}^{i},\mathbf{\chi }_{\mathbf{v},k-1}^{i} \right)

                         \mathbf{z}_{k}^{'}=\sum\limits_{i=0}^{2N}{W_{i}^{(m)}\mathbf{\gamma }_{k|k-1}^{i}}

              ③状态更新方程

                         {​{\mathbf{\Sigma }}_{\mathbf{z},k}}=\sum\limits_{i=0}^{2N}{W_{i}^{(c)}\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right){​{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}

                         {​{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}=\sum\limits_{i=0}^{2N}{W_{i}^{(c)}\left( \mathbf{\chi }_{\theta ,k|k-1}^{i}-\mathbf{\theta }_{k}^{'} \right){​{\left( \mathbf{\gamma }_{k|k-1}^{i}-\mathbf{z}_{k}^{'} \right)}^{T}}}

                         {​{\mathbf{K}}_{k}}={​{\mathbf{\Sigma }}_{\mathbf{\theta z},k}}\cdot \mathbf{\Sigma }_{\mathbf{z},k}^{-1}

                         \left\langle {​{\mathbf{\theta }}_{k}} \right\rangle =\mathbf{\theta }_{k}^{'}+{​{\mathbf{K}}_{k}}({​{\mathbf{z}}_{k}}-\mathbf{z}_{k}^{'})

                         {​{\mathbf{\Sigma }}_{\mathbf{\theta },k}}=\mathbf{\Sigma }_{\mathbf{\theta },k}^{'}-{​{\mathbf{K}}_{k}}{​{\mathbf{\Sigma }}_{\mathbf{z},k}}\mathbf{K}_{k}^{T}

             以上就是扩维UKF滤波算法的全部过程,简化UKF算法的Sigma点数为2n+1,而扩维UKF算法的Sigma点数为2N+1=2(n+m+l)+1。可见,处理加性噪声的简化UKF的Sigma点较处理隐含噪声的扩维UKF要少许多,因此计算量也要更少。

  • 3
    点赞
  • 9
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值