国网法治建设网站,上海装修公司排名榜,移动互联网营销,wordpress 数据库爆炸模拟简单物体二维运动和预测位置
预测数学式
想象一下你正坐在一辆汽车里#xff0c;在雾中行驶。 你几乎看不到路#xff0c;但你有一个 GPS 系统可以告诉你你的速度和位置。 问题是#xff0c;这个 GPS 并不完美#xff1b; 它有时会产生噪音或不准确的读数。 您如何知…模拟简单物体二维运动和预测位置
预测数学式
想象一下你正坐在一辆汽车里在雾中行驶。 你几乎看不到路但你有一个 GPS 系统可以告诉你你的速度和位置。 问题是这个 GPS 并不完美 它有时会产生噪音或不准确的读数。 您如何知道您的实际位置以及行驶速度
卡尔曼滤波器提供了答案。它结合了
系统您的汽车根据其模型预测什么称为预测步骤。它接收到的噪声测量结果在这个类比中是 GPS 读数产生的估计值在统计上比预测或测量本身更可靠。
卡尔曼滤波器主要有两个步骤
预测 x ′ A x B u P ′ A P k − 1 A T Q \begin{aligned} x^{\prime} A xB u \\ P^{\prime} A P_{k-1} A^TQ \end{aligned} x′P′AxBuAPk−1ATQ x ′ x^{\prime} x′是预测状态。 A A A是状态转换模型。 B B B是控制输入模型。 u u u是控制向量。 P ′ P^{\prime} P′是预测估计协方差。 Q Q Q是过程噪声协方差。
更新 y z − H x S H P ′ H T R K P ′ H T S − 1 x x ′ K y P ( I − K H ) P ′ \begin{aligned} y z-H x \\ S H P^{\prime} H^TR \\ K P^{\prime} H^T S^{-1} \\ x x^{\prime}K y \\ P (I-K H) P^{\prime} \end{aligned} ySKxPz−HxHP′HTRP′HTS−1x′Ky(I−KH)P′ y y y是残值 z z z是测量值 H H H是观测模型 S S S是协方差 R R R是测量噪声协方差 K K K是卡尔曼增益 x x x是是更新后的状态估计 P P P是是更新后的估计协方差
代码处理
安装OpenCV和Matplotlib。
使用 OpenCV 实现卡尔曼滤波器
OpenCV 提供了一个方便的 KalmanFilter 类让我们可以实现卡尔曼滤波器而不必陷入数学细节的困境。 在本演示中我们将模拟对象的简单 2D 运动并使用卡尔曼滤波器来估计其位置。
让我们首先初始化 2D 运动的卡尔曼滤波器。
# Initialize the Kalman filter
kalman_2d cv2.KalmanFilter(4, 2)
kalman_2d.measurementMatrix np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
kalman_2d.transitionMatrix np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
kalman_2d.processNoiseCov np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) * 1e-4代码释义
我们系统的状态由 4x1 矩阵表示 [ x , y , x ˙ , y ˙ ] [x, y, \dot{x}, \dot{y}] [x,y,x˙,y˙] 其中 x x x 和 y y y 是二维坐标并且 x ˙ \dot{x} x˙ 和 y ˙ \dot{y} y˙ 分别表示 x 和 y 方向的速度。measurementMatrix 将状态与测量联系起来。在我们的例子中我们只测量位置而不测量速度.。TransitionMatrix 表示状态转换模型。为简单起见我们假设速度恒定。processNoiseCov 代表与我们的流程相关的噪声。
模拟物体运动和可视化
我们将模拟一个沿直线移动的物体并在其测量中添加一些噪声。当物体移动时我们将应用卡尔曼滤波器来估计其真实位置。
我们有 200 个预测状态每个状态都用一个矩阵表示。我们还有 200 个噪声测量每个都由一个2*1 矩阵表示。
让我们可视化对象的真实路径、噪声测量值以及卡尔曼滤波器估计的路径。
fig, ax plt.subplots(figsize(10, 6))
ax.set_xlim(0, 4 * np.pi)
ax.set_ylim(-1.5, 1.5)
ax.set_title(Kalman Filter in 2D Motion Estimation)
ax.set_xlabel(X Position)
ax.set_ylabel(Y Position)# Plotting the true path, noisy measurements, and Kalman filter estimations
ax.plot(true_path[:, 0], true_path[:, 1], g-, labelTrue Path)
ax.scatter(np.array(measurements)[:, 0], np.array(measurements)[:, 1], cred, s20, labelNoisy Measurements)
ax.plot(np.array(predictions)[:, 0, 0], np.array(predictions)[:, 1, 0], b-, labelKalman Filter Estimation)
ax.legend()
plt.show()代码释义
**True Path**这是对象所采取的实际路径尽管我们在现实场景中没有这个路径。**Noisy Measurements**这些是我们从传感器获得的读数这些读数被噪声破坏了。**Kalman Filter Estimations**这些是卡尔曼滤波器估计的位置理想情况下应该接近真实路径。
这是我们模拟的 2D 运动的可视化
预测真实物体的轨迹
跟踪视频人物
二维对象跟踪
源代码
参阅一 - 亚图跨际
参阅二 - 亚图跨际
