KF UKF

我需要Kalman 现在，主要是用来处理检测问题情况里的漏检，因为模拟了一段2D， （x，y）的数据，为了看效果，画的线尽量简单一点：

import numpy as np
import matplotlib.pyplot as pltdef generate_trace(initial_position, velocity, acceleration, num_points, noise_std):x = np.zeros(num_points)y = np.zeros(num_points)x[0], y[0] = initial_positionfor i in range(1, num_points):x[i] = x[i - 1] + velocity[0] + 0.5 * acceleration[0] * i ** 2y[i] = y[i - 1] + velocity[1] + 0.5 * acceleration[1] * i ** 2x[i] += np.random.normal(0, noise_std)y[i] += np.random.normal(0, noise_std)return x, ydef add_missing_points(trace, missing_percentage):num_missing = int(len(trace) * missing_percentage)missing_indices = np.random.choice(len(trace), num_missing, replace=False)print(missing_indices)return missing_indicesinitial_position = (0, 0)
velocity = (5, 3)
acceleration = (0.2, 0.1) 
num_points = 20
noise_std = 10
missing_percentage = 0.3x, y = generate_trace(initial_position, velocity, acceleration, num_points, noise_std)
plt.figure(figsize=(12, 6))
plt.plot(x, y, 'go', label='Original Trace')missing_pos = add_missing_points(x.copy(), missing_percentage)plt.plot(x[missing_pos], y[missing_pos], 'rx', label='Missing Points')x[missing_pos] = np.nan
y[missing_pos] = np.nan
plt.plot(x, y, 'y--', label='Original Trace')
trace = np.vstack((x, y)).T
np.savetxt("car_trace.csv", trace, delimiter=",")plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.title('Traces')
plt.legend()
plt.savefig('car_trace.png')
plt.show()

绿色点：理想情况能检测到每个位置
红色×：假设的漏检情况
接下来就看KF 、 UKF怎么处理我的漏检了，这取决于它们的predict

Prior

Ok，在deep diveUKF & KF 前，有一些参数的definition需要我们的大脑建立一下短暂的存储，我经常分不清字母谁是谁…那我们就全程围绕filterpy这个库吧

class KF:def __init__(self, dt):self.dt = dtself.dim_x = 6  # x, y, vx, vy, ax, ayself.dim_z = 2  # Measurements: x, yself.kf = KalmanFilter(dim_x=self.dim_x, dim_z=self.dim_z)self.kf.F = np.array([[1, 0, dt, 0, 0.5*dt**2, 0],[0, 1, 0, dt, 0, 0.5*dt**2],[0, 0, 1, 0, dt, 0],[0, 0, 0, 1, 0, dt],[0, 0, 0, 0, 1, 0],[0, 0, 0, 0, 0, 1]])self.kf.H = np.array([[1, 0, 0, 0, 0, 0],[0, 1, 0, 0, 0, 0]])self.kf.P *= 1.0q = 0.1self.kf.Q = np.array([[q, 0, 0, 0, 0, 0],[0, q, 0, 0, 0, 0],[0, 0, q, 0, 0, 0],[0, 0, 0, q, 0, 0],[0, 0, 0, 0, q, 0],[0, 0, 0, 0, 0, q]])r = 0.5**2self.kf.R = np.array([[r, 0],[0, r]])

class EUKF:def __init__(self, dt):self.dt = dtself.dim_x = 6  # x, y, vx, vy, ax, ayself.dim_z = 2  # Measurements: x, yself.points = MerweScaledSigmaPoints(n=self.dim_x, alpha=0.001, beta=2.0, kappa=0)self.ukf = UKF(dim_x=self.dim_x, dim_z=self.dim_z, fx=self.f_process, hx=self.h_measurement,dt=self.dt, points=self.points)self.ukf.P = np.diag([1.0, 1.0, 100.0, 100.0, 1000.0, 1000.0])self.ukf.Q = np.diag([0.1, 0.1, 0.1, 0.1, 0.05, 0.05])self.ukf.R = np.diag([0.5**2, 0.5**2])

两个算法的common 参数如下：
dim_x : 这个代表你的feature维度，这个看需要设置，比如我的states是：x, y, vx, vy, ax, ay，那么这里就是6
dim_z：这个代表你的目标输出维度，比如我需要追踪，那么位置（x,y）是我的目标输出，这里就是2
dt：这个代表standard unit，物理上为1（我们常说的每秒）个单位长度，在CV处理上，我们是1秒30帧，因为处理的是图像信息，所以我们的标准单位应该是基于frame的秒，1个frame为1/30 秒的处理单位时长,这里一般赋值1/30

以下三个大写字母都是各种covariance
P：covariance estimate matrix表示对状态估计的不确定性

Q：process noise matrix影响滤波器在预测步骤中对过程模型的信任程度。较大的Q值表示滤波器认为过程模型的不确定性较大，从而对预测结果的置信度降低

R：measurement noise matrix表示测量中的噪声,较大的R值表示滤波器认为测量数据的不确定性较大，从而对测量更新的影响减小

Tips：

通常来说，P在用UKF/KF 过程，用不停的输入来调整自己。而Q，R是初始后就不变的。

KF：使用线性状态转移矩阵 F。状态预测由矩阵乘法完成
UKF：使用非线性状态转移函数 f_process。通过对 sigma 点进行无迹变换，实现状态预测。