无迹卡尔曼滤波算法_自动驾驶相关算法之扩展卡尔曼滤波

最新推荐文章于 2024-08-18 10:30:33 发布

weixin_39821035

最新推荐文章于 2024-08-18 10:30:33 发布

阅读量257

点赞数

文章标签：无迹卡尔曼滤波算法

扩展卡尔曼滤波

主要思路：将递推公式用一阶泰勒展开近似，缺点也比较明显，某些情况下，局部线性化并不能很好的逼近真实值。

对于某系统模型有： $$ mathbb x_t = g(mathbb x_{t-1}, mathbb u_t) + e_t $$ 其中，$e_t$表示过程误差。

对于观测模型有： $$ mathbb z_t = h(mathbb x_t) + delta_t $$ 其中，$delta_t$是观测误差。

函数$g(mathbb x_{t-1}, mathbb u_t)和h(mathbb x_t)$可以是非线性函数，所以需要先将这两个函数做泰勒展开，有： $$ g(mathbb x_{t-1}, mathbb u_t) approx g(mathbb mu_{t-1}, mathbb u_t) + g'(mathbb mu_{t-1}, mathbb u_t) (mathbb x_{t-1} - mathbb mu_{t-1}) = g(mathbb mu_{t-1}, mathbb u_t) + G_t (mathbb x_{t-1} - mathbb mu_{t-1}) $$

$$ h(mathbb x_t) approx h(mathbb mu_t) + h'(mathbb mu_t) (mathbb x_t - mathbb mu_t) = h(mathbb mu_t) + H_t (mathbb x_t - mathbb mu_t) $$

其中，$G_t, H_t$分别表示预测模型和观测模型对状态的一阶导(雅克比矩阵)。

预测 $$ mathbb x_t = g(mathbb x_{t-1}, mathbb u_t) P_t = G_tP_tG_t^T + Q_t $$ 更新 $$ Residual_t = mathbb z_t - h(mathbb x_t) S = H_t P_t H_t^T + R_t K_t = P_t H_t^T S^{-1} mathbb x_t = mathbb x_t + K_t Residual_t P_t = P_t - K_t H_t P_t $$

无迹卡尔曼滤波

主要思路：采样数值方法。因为系统比较复杂，直接采用蒙特卡罗方法，随机样本模拟，然后统计样本的均值和方差作为运动后的均值和方差。

关键问题：如何选取样本？

无迹变换(Unscented Transform)

核心思想：近似一种概率分布比近似任意一个非线性函数或非线性变换容易。

假设n维变量$mathbb x$ 服从某均值为$mathbb mu$协方差为$P_x$的分布，变量经过某变换$mathbb f$，得到$mathbb y$，有$mathbb y = mathbb f(mathbb x)$求$mathbb y$的均值和分布。

UT变换算法如下：

根据$mathbb mu, P$采用一定的采样策略获得$sigma$点集${chi_i}$。 $$ chi_0 = mathbb mu $$
$$ chi_i = mathbb mu + (sqrt{(n + lambda) P_x})_i quad i = 1 cdots n $$
$$ chi_{i+n} = mu - (sqrt{(n + lambda) P_x})_i quad i = 1 cdots n $$
$w_i^m，w_i^c$分别是均值和方差的权重。第一个点的均值权重和方差权重为： $$ w_0^m = frac {lambda} { n + lambda} w_0^c = frac {lambda} {n + lambda} + 1 - alpha^2 + beta $$ 剩下点的均值权重和方差权重为： $$ w_i^m = w_i^c = frac {1} {2(n + lambda)} quad i = 1 cdots 2n $$ $alpha, beta, lambda$是输入的常量，也是需要调整的参数， $$ alpha in (0, 1] beta = 2 (高斯分布时的最优取值) lambda = alpha^2(n + k) - n k ge 0 $$ 其中$k$影响着采样点到均值点的距离。
对$sigma$中的每个点分别做变换得到点集${mathbb y_i}$
对点集$mathbb y_i$，依据权重分别计算均值和方差，$bar {mathbb y}, P_y$， $$ bar {mathbb y} = Sigma_{i=0}^{2n} w_i^{m} mathbb y_i P_y = Sigma_{i=0}^{2n} w_i^c (mathbb y_i - bar {mathbb y}) (mathbb y_i - bar {mathbb y})^T $$

UKF算法过程

预测

根据UT变换生成$sigma$点集$Chi_{t-1}$
对点集$Chi_{t-1}$中的每一个点分别执行$g(u_t, chi_i)$
根据式(13)计算点集均值和方差。

更新

根据UT变换生成sigma点集$Psi_{t}$
对点集$Psi_t$中的每一个点执行观测变换函数$h(psi_i)$,得到点集$mathcal Z_t$
计算点集$mathcal Z_t$的均值和协方差矩阵 $$ bar {mathcal z_t} = Sigma_{i = 0} ^ {2n} w_i^{m} Z_t^i S_t = Sigma_{i=0} ^ {2n} w_i^c (Z_t^i - bar {mathcal z_t}) (Z_t^i - bar {mathcal z_t}) ^ T + Q_t $$
计算x到z的变换矩阵 $$ P_t^{x, z} = Sigma_{i=0}^{2n} w_i^c (Psi_t^i - mathbb mu_t) (Z_t^i - bar {mathcal z_t})^T $$
计算增益$K_t = P_t^{x, z} S_t$
更新均值和方差 $$ mu_t = mu_t + K_t (z_t - bar {mathcal z_t}) P_t = P_t - K_tS_tK_t^T $$

应用

车辆在低速状况下可以用如下模型描述： $$ x_{t + 1} = x_t + v_t cos(heta_t) y_{t + 1} = y_t + v_t sin(heta_t) heta_{t+1} = heta_t + delta dt v_{t+1} = a $$ 写成矩阵乘法的形式： $$ mathbb x_{t +1} = begin{pmatrix}

quad 0. quad 0. quad 0.
quad 1. quad 0. quad 0.
quad 0. quad 1. quad 0.
quad 0. quad 0. quad 0. end{pmatrix} mathbb x_t + begin{pmatrix} dt imes cos(heta_t) quad 0 dt imes sin(heta_t) quad 0 0 quad dt 1.0 quad 0 end {pmatrix} mathbb u_t $$

这里简化了模型的处理，$delta, a$是控制输入。车上携带了一个可以输出定位信息的传感器，要求能实时估计估计车辆的位姿。

这里先给出车辆仿真部分的代码

class Car: def __init__(self, x = 0.0, y = 0.0, yaw=0.0, v = 0.0, L=1.0): self.x = x self.y = y self.yaw = yaw self.v = v self.L = L self.observe_variance = np.diag([0.4, 0.4]) ** 2 self.process_variance = np.diag([1.0, math.radians(10)]) ** 2 self.a = 1.0 self.sigma = 0.1 def _calc_input(self): self.a = 1.0 self.sigma = 0.1 return self.a, self.sigma def move(self, dt=0.01): a, sigma = self._calc_input() self.x = self.x + self.v * math.cos(self.yaw) * dt self.y = self.y + self.v * math.sin(self.yaw) * dt self.yaw = self.yaw + sigma * dt self.v = a def observe(self): x = self.x + np.random.randn() * self.observe_variance[0, 0] y = self.y + np.random.randn() * self.observe_variance[1, 1] return x, y def get_input(self): a = self.a + np.random.randn() * self.process_variance[0, 0] sigma = self.sigma + np.random.randn() * self.process_variance[1, 1] return a, sigma def state(self): return self.x, self.y, self.yaw, self.v

扩展卡尔曼滤波

前面讨论过扩展卡尔曼滤波主要是对预测函数$g(mathbb x_{t}, mathbb u_t)$和观测函数$h(mathbb x_{t})$做展开。

根据模型，求导有 $$ G_t = begin{pmatrix} 1.0 quad 0.0 quad - dt imes sin(heta_t) quad dt imes cos(heta_t) 0.0 quad 1.0 quad dt imes cos(heta_t) quad dt imes sin(heta_t) 0.0 quad 0.0 quad 1.0 quad 0.0 0.0 quad 0.0 quad 0.0 quad 1.0 end{pmatrix} $$

$$ H_t = begin {pmatrix} 1 quad 0 quad 0 quad 0 0 quad 1 quad 0 quad 0 end {pmatrix} $$

具体实现代码

class PositionEstimator: def __init__(self, initial_state, initial_P): """ :param initial_state: """ self.X = initial_state self.P = initial_P def jacob_F(self, x, u, DT=0.01): """ :param x: :param u: :return: """ yaw = x[2, 0] v = u[0, 0] j_f = np.matrix([ [1.0, 0.0, - DT * math.sin(yaw), DT*math.cos(yaw)], [0.0, 1.0, DT * math.cos(yaw), DT*math.sin(yaw)], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0] ]) return j_f def prediction(self, u, Q, DT=0.01): F = np.matrix(''' 1. 0. 0. 0.; 0. 1. 0. 0.; 0. 0. 1. 0.; 0. 0. 0. 0.''') B = np.matrix([[DT * math.cos(self.X[2, 0]), 0], [DT * math.sin(self.X[2, 0]), 0], [0.0, DT], [1.0, 0]]) X_prd = F * self.X + B * u j_f = self.jacob_F(self.X, u, DT=DT) self.P = j_f * self.P * j_f.T + Q self.X = X_prd return self.X, self.P def jacob_H(self, x): j_h = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0]]) return j_h def estimation(self, z, R): j_h = self.jacob_H(self.X) y = z - j_h * self.X S = j_h * self.P * j_h.T + R K = self.P * j_h.T * np.linalg.inv(S) self.X = self.X + K * y self.P = (np.eye(self.X.shape[0]) - K * j_h) * self.P return self.X, self.P def ekf_estimation(self, u, z, Q, R, DT=0.01): self.prediction(u, Q, DT=DT) self.estimation(z, R) return self.X, self.P

无迹卡尔曼滤波

class UKFPoseEstimator(object): def __init__(self, initial_state, initial_p, ALPHA, BETA, KAPPA): self.X = initial_state self.P = initial_p self.wm, self.wc, self.gamma = self._setup(self.X.shape[0], ALPHA, BETA, KAPPA) def _setup(self, n, ALPHA, BETA, KAPPA): lamda = ALPHA ** 2 * (n + KAPPA) - n wm = [lamda / (lamda + n)] wc = [lamda / (lamda + n) + (1 - ALPHA ** 2 + BETA)] for i in range(2 * n): wm.append(1.0 / (2 * (n + lamda))) wc.append(1.0 / (2 * (n + lamda))) gamma = math.sqrt(n + lamda) wm = np.matrix(wm) wc = np.matrix(wc) return wm, wc, gamma def _generate_sigma_points(self, gamma): sigma = self.X P_sqrt = np.matrix(scipy.linalg.sqrtm(self.P)) n = self.X.shape[0] for i in range(n): sigma = np.hstack((sigma, self.X + gamma * P_sqrt[:, i])) for i in range(n): sigma = np.hstack((sigma, self.X - gamma * P_sqrt[:, i])) return sigma def _predict_motion(self, sigma, u): for i in range(sigma.shape[1]): sigma[:, i] = self._motion_model(sigma[:, i], u) return sigma def _motion_model(self, X, u, DT=0.1): F = np.matrix(''' 1. 0. 0. 0.; 0. 1. 0. 0.; 0. 0. 1. 0.; 0. 0. 0. 0. ''') B = np.matrix([[DT * math.cos(X[2,0]), 0], [DT * math.sin(X[2,0]), 0], [0, DT], [1, 0]]) X_prd = F * X + B * u return X_prd def _observation_model(self, X): H = np.matrix(''' 1. 0. 0. 0.; 0. 1. 0. 0. ''') return H * X def _calc_sigma_covariance(self, X, sigma, Q): nSigma = sigma.shape[1] d = sigma - X[0:sigma.shape[0], :] P = Q for i in range(nSigma): P = P + self.wc[0, i] * d[:, i] * d[:, i].T return P def _calc_p_xz(self, sigma, x, z_sigma, zb): nSigma = sigma.shape[1] dx = np.matrix(sigma - x) dz = np.matrix(z_sigma - zb[0:2, :]) P = np.matrix(np.zeros((dx.shape[0], dz.shape[0]))) for i in range(nSigma): P = P + self.wc[0, i] * dx[:, i] * dz[:, i].T return P def predict(self, u, Q, DT=0.1): sigma = self._generate_sigma_points(self.gamma) for i in range(sigma.shape[1]): sigma[:, i] = self._motion_model(sigma[:, i], u, DT=DT) self.X = (self.wm * sigma.T).T self.P = self._calc_sigma_covariance(self.X, sigma, Q) return self.X, self.P def estimation(self, z, R): z_pred = self._observation_model(self.X) y = z - z_pred # calculate K sigma = self._generate_sigma_points(self.gamma) # mean zb = (self.wm * sigma.T).T # to observation points for i in range(sigma.shape[1]): sigma[0:2, i] = self._observation_model(sigma[:, i]) z_sigma = sigma[0:2, :] st = self._calc_sigma_covariance(zb, z_sigma, R) Pxz = self._calc_p_xz(sigma, self.X, z_sigma, zb) K = Pxz * np.linalg.inv(st) self.X = self.X + K * y self.P = self.P - K * st * K.T return self.X, self.P

结果