多TOA观测移动目标定位仿真：EKF、UKF、PF、EKPF解算比较-编程实验室

【19】多toa观测移动目标定位仿真 ekf ukf pf ekpf解算比较

在移动目标定位领域，基于到达时间（TOA）的定位方法是一种常见且有效的手段。今天咱们就来深入探讨一下使用扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、粒子滤波（PF）以及扩展卡尔曼粒子滤波（EKPF）对基于多TOA观测的移动目标进行定位仿真，并比较它们的解算性能。

一、TOA定位基本原理

TOA定位是通过测量信号从移动目标发送到多个基站接收的时间差，进而计算出目标位置。假设目标位置为$(x, y)$，基站$i$的位置为$(xi, yi)$，信号传播速度为$c$，那么根据TOA测量值$t_i$，我们有以下距离方程：

$\sqrt{(x - xi)^2 + (y - yi)^2} = c \cdot t_i$

二、EKF（扩展卡尔曼滤波）

EKF是一种常用的非线性滤波方法，它通过对非线性系统进行一阶泰勒展开来近似线性化。在TOA定位中，系统状态方程和观测方程一般是非线性的。

状态方程

假设目标的状态为$\mathbf{x} = [x, y, \dot{x}, \dot{y}]^T$，状态转移方程可以写成：

$\mathbf{x}{k + 1} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \mathbf{x}k + \mathbf{w}_k$

其中$\Delta t$是时间间隔，$\mathbf{w}_k$是过程噪声。

观测方程

观测方程为：

【19】多toa观测移动目标定位仿真 ekf ukf pf ekpf解算比较

$zi = \sqrt{(x - xi)^2 + (y - yi)^2} + vi$

这里$zi$是第$i$个基站的TOA观测值，$vi$是观测噪声。

在代码实现中，EKF主要步骤如下：

import numpy as np # 初始化EKF参数 def ekf_init(): # 状态转移矩阵 F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # 观测矩阵（这里需要根据实际情况线性化计算） H = np.zeros((num_anchors, 4)) Q = np.diag([q1, q2, q3, q4]) # 过程噪声协方差 R = np.diag([r1, r2, r3]) # 观测噪声协方差 x_hat = np.zeros((4, 1)) # 初始状态估计 P = np.eye(4) # 初始估计协方差 return F, H, Q, R, x_hat, P # EKF预测步骤 def ekf_predict(x_hat, P, F, Q): x_hat = F.dot(x_hat) P = F.dot(P).dot(F.T) + Q return x_hat, P # EKF更新步骤 def ekf_update(x_hat, P, H, z, R): y = z - H.dot(x_hat) # 残差 S = H.dot(P).dot(H.T) + R # 观测预测协方差 K = P.dot(H.T).dot(np.linalg.inv(S)) # 卡尔曼增益 x_hat = x_hat + K.dot(y) P = (np.eye(4) - K.dot(H)).dot(P) return x_hat, P

EKF分析

EKF通过线性化处理非线性问题，优点是计算量相对较小，在非线性程度不高的情况下能有较好的估计效果。然而，一阶泰勒展开会引入线性化误差，当系统非线性较强时，估计精度会受到影响。

三、UKF（无迹卡尔曼滤波）

UKF采用了一种确定性采样策略——Sigma点采样，避免了EKF中的线性化过程，能够更好地处理非线性问题。

Sigma点采样

首先确定Sigma点集，然后通过状态转移方程传播Sigma点，再计算预测均值和协方差。

# UKF初始化 def ukf_init(): # 状态转移矩阵同EKF F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) Q = np.diag([q1, q2, q3, q4]) R = np.diag([r1, r2, r3]) x_hat = np.zeros((4, 1)) P = np.eye(4) # UKF参数 n = 4 # 状态维度 lambda_ = alpha**2 * (n + kappa) - n Wm = np.zeros(2 * n + 1) Wc = np.zeros(2 * n + 1) Wm[0] = lambda_ / (n + lambda_) Wc[0] = lambda_ / (n + lambda_) + (1 - alpha**2 + beta) for i in range(1, 2 * n + 1): Wm[i] = 1 / (2 * (n + lambda_)) Wc[i] = 1 / (2 * (n + lambda_)) return F, Q, R, x_hat, P, lambda_, Wm, Wc # UKF预测步骤 def ukf_predict(x_hat, P, F, Q, lambda_, Wm, Wc): n = 4 Xsig = np.zeros((n, 2 * n + 1)) Xsig[:, 0] = x_hat.flatten() sqrt_P = np.linalg.cholesky((n + lambda_) * P) for i in range(n): Xsig[:, i + 1] = x_hat.flatten() + sqrt_P[:, i] Xsig[:, i + n + 1] = x_hat.flatten() - sqrt_P[:, i] Xsig = F.dot(Xsig) x_hat = np.zeros((4, 1)) for i in range(2 * n + 1): x_hat += Wm[i] * Xsig[:, i].reshape(-1, 1) P = np.zeros((4, 4)) for i in range(2 * n + 1): P += Wc[i] * (Xsig[:, i].reshape(-1, 1) - x_hat).dot((Xsig[:, i].reshape(-1, 1) - x_hat).T) P += Q return x_hat, P # UKF更新步骤 def ukf_update(x_hat, P, R, Z, lambda_, Wm, Wc): n = 4 Xsig = np.zeros((n, 2 * n + 1)) Xsig[:, 0] = x_hat.flatten() sqrt_P = np.linalg.cholesky((n + lambda_) * P) for i in range(n): Xsig[:, i + 1] = x_hat.flatten() + sqrt_P[:, i] Xsig[:, i + n + 1] = x_hat.flatten() - sqrt_P[:, i] Zsig = np.zeros((num_anchors, 2 * n + 1)) for i in range(2 * n + 1): Zsig[:, i] = calculate_measurement(Xsig[:, i]) z_hat = np.zeros((num_anchors, 1)) for i in range(2 * n + 1): z_hat += Wm[i] * Zsig[:, i].reshape(-1, 1) Pzz = np.zeros((num_anchors, num_anchors)) Pxz = np.zeros((n, num_anchors)) for i in range(2 * n + 1): Pzz += Wc[i] * (Zsig[:, i].reshape(-1, 1) - z_hat).dot((Zsig[:, i].reshape(-1, 1) - z_hat).T) Pxz += Wc[i] * (Xsig[:, i].reshape(-1, 1) - x_hat).dot((Zsig[:, i].reshape(-1, 1) - z_hat).T) Pzz += R K = Pxz.dot(np.linalg.inv(Pzz)) y = Z - z_hat x_hat = x_hat + K.dot(y) P = P - K.dot(Pzz).dot(K.T) return x_hat, P

UKF分析

UKF通过更准确地逼近非线性函数的均值和协方差，在处理强非线性系统时表现优于EKF。但它的计算量比EKF大，因为需要处理Sigma点的传播。

四、PF（粒子滤波）

粒子滤波基于蒙特卡罗方法，通过大量粒子来近似系统状态的概率分布。

粒子初始化

首先随机生成大量粒子，并赋予每个粒子相同的权重。

# 粒子滤波初始化 def pf_init(num_particles): particles = np.zeros((4, num_particles)) for i in range(num_particles): particles[:, i] = np.random.multivariate_normal(mean=[0, 0, 0, 0], cov=np.eye(4)) weights = np.ones(num_particles) / num_particles return particles, weights # 粒子预测 def pf_predict(particles, F, Q, dt): for i in range(particles.shape[1]): particles[:, i] = F.dot(particles[:, i].reshape(-1, 1)).flatten() + np.random.multivariate_normal(mean=[0, 0, 0, 0], cov=Q).flatten() return particles # 粒子权重更新 def pf_update(particles, weights, z, R): for i in range(particles.shape[1]): measurement_predicted = calculate_measurement(particles[:, i]) weights[i] *= np.exp(-0.5 * np.linalg.inv(R).dot((z - measurement_predicted).reshape(-1, 1)).T.dot((z - measurement_predicted).reshape(-1, 1))) weights = weights / np.sum(weights) return weights # 重采样 def pf_resample(particles, weights): indices = np.random.choice(np.arange(particles.shape[1]), size=particles.shape[1], p=weights) particles = particles[:, indices] weights = np.ones(particles.shape[1]) / particles.shape[1] return particles, weights

PF分析

PF理论上可以处理任意非线性非高斯系统，具有很强的适应性。然而，它的性能依赖于粒子数量，粒子数过少会导致估计不准确，而粒子数过多又会使计算量急剧增加。

五、EKPF（扩展卡尔曼粒子滤波）

EKPF结合了EKF和PF的优点，先用EKF对粒子进行初步估计，再利用PF进行更精确的修正。

# EKPF初始化 def ekpf_init(num_particles): particles, weights = pf_init(num_particles) F, H, Q, R, x_hat, P = ekf_init() return particles, weights, F, H, Q, R, x_hat, P # EKPF预测更新 def ekpf_predict_update(particles, weights, z, F, H, Q, R, x_hat, P): for i in range(particles.shape[1]): x_hat_particle = particles[:, i].reshape(-1, 1) P_particle = np.eye(4) x_hat_particle, P_particle = ekf_predict(x_hat_particle, P_particle, F, Q) x_hat_particle, P_particle = ekf_update(x_hat_particle, P_particle, H, z, R) particles[:, i] = x_hat_particle.flatten() weights = pf_update(particles, weights, z, R) particles, weights = pf_resample(particles, weights) return particles, weights