Created by potrace 1.13, written by Peter Selinger 2001-2015

无损卡尔曼滤波器-UKF

CTRV模型

在卡尔曼滤波器的模型中,速度作为常量被保存在状态向量中:

x = \begin{bmatrix} p\\ v \end{bmatrix}

这就导致了不能预测速度的方向,也就是不能预测转弯,卡尔曼滤波器中的转弯是靠传感器的纠正.

为了解决这个问题我们使用CTRV模型:

image

使用位置 p_x , p_y ,速度 v ,角度 \psi ,及角速度 \dot{\psi} 来作为状态向量:

x = \begin{bmatrix} p_x\\ p_y\\ v\\ \psi\\ \dot{\psi} \end{bmatrix}

过程模型

虽然状态向量的变化时非线性的,但是我们可以使用积分准确的求出下一时刻的状态向量

x_{k+1} = x_k + \int_{t_k}^{t_{k+1}}\begin{bmatrix} \dot{p_x}\\ \dot{p_y}\\ \dot{v}\\ \dot{\psi}\\ \ddot{\psi} \end{bmatrix}dt

计算积分:

x_{k+1} = x_k + \begin{bmatrix} \frac{v_k}{\dot{\psi_k}}(sin(\psi_k + \dot{\psi_k}\Delta t) - sin(\psi_k))\\ \frac{v_k}{\dot{\psi_k}}(-cos(\psi_k + \dot{\psi_k}\Delta t) + cos(\psi_k))\\ 0\\ \dot{\psi_k}\Delta t\\ 0 \end{bmatrix}

接下来我们需要加入噪声:

v_k = \begin{bmatrix} v_{a,k}\\ v_{\ddot{\psi},k} \end{bmatrix}

其中 v_{a,k} 与 v_{\ddot{\psi},k} 都符合正态分布.

将噪声结合到过程模型中:

x_{k+1} = x_k + \begin{bmatrix} \frac{v_k}{\dot{\psi_k}}(sin(\psi_k + \dot{\psi_k}\Delta t) - sin(\psi_k))\\ \frac{v_k}{\dot{\psi_k}}(-cos(\psi_k + \dot{\psi_k}\Delta t) + cos(\psi_k))\\ 0\\ \dot{\psi_k}\Delta t\\ 0 \end{bmatrix} + \begin{bmatrix} \frac{1}{2}(\Delta t)^2 cos(\psi_k) \cdot \nu_{a,k}\\ \frac{1}{2}(\Delta t)^2 sin(\psi_k) \cdot \nu_{a,k}\\ \Delta t \cdot \nu_{a,k}\\ \frac{1}{2}(\Delta t)^2 \cdot \nu_{\ddot{\psi},k}\\ \Delta t \cdot \nu_{\ddot{\psi},k} \end{bmatrix} \tag{1}

非线性协方差矩阵的计算

我们来回顾一下卡尔曼滤波器与EKF的协方差矩阵是如何计算的:

卡尔曼滤波器是线性模型.根据状态向量是成比例可以推导出协方差矩阵也是成比例的所以:

\mathbf{P}_k = \mathbf{F_k} \mathbf{P}_{k-1} \mathbf{F}_k^T

扩展卡尔曼滤波器EKF用泰勒公式的第一项,也就是最接近非线性曲线斜率的一条直线来模拟非线性.由于是用直线模拟的曲线,所以协方差矩阵也可以按照线性模型的方法进行计算.只不过线性模型中状态向量的比例替换为泰勒公式中的雅可比矩阵.

扩展卡尔曼滤波器因为采用了线性化,所以有精度损失,有没有办法不用线性化直接计算出非线性的协方差矩阵?

image

无损卡尔曼滤波器UKF采用了一个sigma算子的方案解决了这个问题:
首先,计算前一次协方差矩阵的sigma算子,sigma算子其实就是几个有代表性的点.

image

然后将这些sigma算子带入到过程模型中得到非线性后的sigma算子.

image

最后根据这些算计计算出协方差矩阵.

image

我们发现无损卡尔曼滤波器也不是100%准确的通过计算得到的结果,而是根据几个有代表性的点的变化,重新生成了一个协方差矩阵.如果这几个点取的合理的话无损卡尔曼滤波器的误差是非常小的.

选取算子

首先要确定我们选取算子的个数,通常我们会选择 2n+1 个算子,n为状态向量的纬度.

image

如图,中间的点是状态向量,然后在对称的在周围取几个点,所以个数为 2n+1 .

算子的公式是这样的:

X_{k|k} = \Bigg [ x_{k|k} \qquad x_{k|k}+\sqrt{(\lambda+n_x)P_{k|k}} \qquad x_{k|k}-\sqrt{(\lambda+n_x)P_{k|k}} \Bigg]

其中 X_{k|k} 是算子, x_k 是状态向量, n 为状态向量的纬度, \lambda 可以决定周围的点离中心的距离,通常取 \lambda = 3 - n_x .公式中 x_{k|k} 为状态向量, x_{k|k}+\sqrt{(\lambda+n_x)P_{k|k}} 与 x_{k|k}-\sqrt{(\lambda+n_x)P_{k|k}} 是对称的两个点,通常有n对.

噪声

在预测之前我们还需要考虑噪声.

v_k = \begin{bmatrix} v_{a,k}\\ v_{\ddot{\psi},k} \end{bmatrix}

噪声也是非线性的.无损卡尔曼滤波给出的解决方案是扩充状态向量,这样的话协方差矩阵也会跟着扩充.

x_{a,k} = \begin{bmatrix} p_x\\ p_y\\ v\\ \psi\\ \dot{\psi}\\ \nu_a\\ \nu_{\ddot{\psi}} \end{bmatrix}

P_{a,k|k} = \begin{bmatrix} P_{k|k} \quad 0 \\ 0 \qquad Q \end{bmatrix}

预测算子

预测步骤非常简单,只要将我们计算好的算子 X 带入到过程模型即方程(1)中即可得到预测后的算子.

计算协方差矩阵

根据预测后的算子计算状态向量:
x_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i X_{k+1|k,i}

根据预测后的算子计算协方差矩阵:
P_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i( X_{k+1|k,i} - x_{k+1|k})(X_{k+1|k,i} - x_{k+1|k})^T

其中的w为是什么?我们可以这样理解:
我们在生成sigma算子的时候,通过分散值调整了sigma距离状态向量的距离

image

现在我们需要根据sigma算子计算出状态向量与协方差,所以我们需要这个分散值的逆

image

w_i =\frac{\lambda}{\lambda+n_{a}}, i =1

w_i =\frac{1}{2(\lambda+n_{a})}, i =2...n_{a}

状态向量转换到测量值

接下来我们需要把状态向量
x_{k+1|k}=\begin{bmatrix} p_x\\ p_y\\ v\\ \psi\\ \dot{\psi} \end{bmatrix}
转换成测量向量
z_{k+1|k}=\begin{bmatrix} \rho\\ \varphi\\ \dot{\rho} \end{bmatrix}
以便后续与测量的数据进行计算.

依然用sigma算子,根据几何关系将sigma算子转换成测量向量.
z_{k+1|k}=h(x_{k+1}) + w_{k+1}

\rho = \sqrt{p_x^2+p_y^2}

\varphi =arctan(\frac{p_y}{p_x})

\dot{\rho}=\frac{p_xcos(\psi)v+p_ysin(\psi)v}{\sqrt{p_x^2+p_y^2}}

然后再计算出转换后的状态向量:
x_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i X_{k+1|k,i}

计算测量数据的协方差矩阵:

S_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i( Z_{k+1|k,i} - z_{k+1|k})(Z_{k+1|k,i} - z_{k+1|k})^T + R

R = E(w_k\cdot w_k^T) = \begin{bmatrix} \sigma_{\rho}^2 \qquad 0\qquad0\\ 0\qquad\sigma_{\varphi}^2 \qquad 0\\ 0\qquad0\qquad\sigma_{\dot{\rho}}^2 \end{bmatrix}

更新

更新部分几乎其实和卡尔曼滤波器一样:

x_{k+1|k+1} = x_{k+1|k}+K_{k+1|k}(z_{k+1}-z_{k+1|k})

P_{k+1|k+1} = P_{k+1|k}-K_{k+1|k}S_{k+1|k}K^T_{k+1|k}

有所不同的地方是卡尔曼增益的计算:

K_{k+1|k} = T_{k+1|k}S^{-1}_{k+1|k}

这里的T是状态向量空间与测量向量空间相互关联的矩阵:

T_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i (X_{k+1|k,i} - x_{k+1|k})\ (Z_{k+1|k,i} - z_{k+1|k})^T

滤波器一致性检查(NIS)

我们在引入过几个噪声:

过程噪声 V_k ,及噪声的协方差矩阵 Q .

测量时也引入测量噪声 w_k ,及协方差矩阵 R .

image

其中测量误差我们可以在传感器的说明书上找到.而模型的过程误差需要我们自己来指定.那么如何确定过程误差呢?

我们可以通过NIS的值来确定误差是否合理:
\varepsilon = (z_{k+1}-z_{k+1|k})^TS^{-1}_{k+1|k}(z_{k+1}-z_{k+1|k})

NIS的全称是归一化新息平方.新息是预测测量值之和与实际测量值之差.归一化是指相对S归一化.

需要注意的是NIS的结果是一个数字,它成卡方分布.

posted @ 2018-07-12 10:06:39
评论加载中...
发表评论