自动驾驶（十三）---------无损卡尔曼滤波

最新推荐文章于 2024-07-23 20:36:30 发布

一实相印

最新推荐文章于 2024-07-23 20:36:30 发布

阅读量1.6w

点赞数 3

分类专栏：自动驾驶

本文链接：https://blog.csdn.net/zhouyy858/article/details/98632684

版权

自动驾驶专栏收录该内容

87 篇文章 1096 订阅

订阅专栏

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

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

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

通过上面的介绍，出现几个问题：

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

1.关键点怎么找

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

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

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

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

通过状态转移得到新的点，下面计算新的状态：

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

均值计算方式： $\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]$

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

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

（1）初始化：

$\left\langle {{\mathbf{\theta }}_{0}} \right\rangle =E\left[ {{\mathbf{\theta }}_{0}} \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要少许多，因此计算量也要更少。