我有一个移动汽车的小数据集:
Data_=[time x,y,z]; %# ONLY THIS DATA
我知道在这种情况下,速度和加速度不是恒定的。
我想估计不同时间的汽车位置。我决定使用卡尔曼滤波器。我搜索了卡尔曼滤波器,但找不到用于在 3D 空间中以速度和加速度跟踪对象的代码。我不知道从哪里开始。卡尔曼滤波器可以自动处理速度和加速度吗?
有人可以帮助我并提供一些链接或指导吗?
我有一个移动汽车的小数据集:
Data_=[time x,y,z]; %# ONLY THIS DATA
我知道在这种情况下,速度和加速度不是恒定的。
我想估计不同时间的汽车位置。我决定使用卡尔曼滤波器。我搜索了卡尔曼滤波器,但找不到用于在 3D 空间中以速度和加速度跟踪对象的代码。我不知道从哪里开始。卡尔曼滤波器可以自动处理速度和加速度吗?
有人可以帮助我并提供一些链接或指导吗?
我的建议是去Mathworks 文件交换并搜索卡尔曼滤波器
你会发现这个非常标准的算法的好几段代码。
就卡尔曼滤波器本身而言,它们就是所谓的预测器-估计器。也就是说,他们可以在n
给定时间的观察结果的情况下对时间的状态进行预测n-1
。然后,在您收到 time 的观察结果后,n
您可以对 time 之前的所有时间进行估计(有人称之为平滑)n
。估计部分是通过所谓的创新和当前的卡尔曼增益来完成的。
卡尔曼滤波器通过“状态空间”的概念工作,即您的状态存储有关对象的所有必要信息。不同的观察向量是您可以观察到的系统。例如,在恒定加速度模型中,您可能会假设状态仅包含 3 个位置值和 3 个速度值(x、y 和 z)。过滤器的设计者负责决定状态空间和状态转换模型(您希望在没有观察的情况下状态如何变化。)
您必须选择一个状态转换矩阵,您必须了解观察误差的协方差矩阵,状态转换矩阵中误差的协方差矩阵(即,您的状态转换有多好)模型是),以及初始状态估计的协方差矩阵(您还必须选择)。您还必须选择状态向量和观察向量之间的关系。
如果您假设高斯观测噪声、高斯过程噪声和其他一些标准事物,卡尔曼滤波器是最大似然最优线性估计器。
查看 udacity.com 的 AI 课程,CS373。他们在那里很好地解释了卡尔曼滤波器。
Kalman Filter
在一个循环中是 5-6 行。您不需要任何人的实施。
您需要的是一个描述汽车轨迹的线性系统模型。如果你有系统矩阵A,B,C
(或F,G,H
),你实际上已经完成了。
Kalman Filter
是一种通用的Bayesian
过滤算法。它适用于任何线性高斯情况。
计算机视觉系统工具箱现在有一个vision.KalmanFilter
对象。这是一个如何使用它来跟踪对象的示例。该示例是 2D 的,但可以很容易地推广到 3D。
在您的情况下,(线性)卡尔曼滤波器可能是最好的选择(它的第一个应用实际上是跟踪阿波罗太空船的位置以正确撞击月球!)。所以有很多关于这个问题的教程,例如看这个可爱的小机器人的例子。它实际上是大约 5 行代码(请注意,您应该使用持久变量)。协方差矩阵(通常是P、R、Q)的调整是有根据的猜测。将 P 初始化为对角矩阵 P= eye(length(x))*1e3
并选择噪声矩阵R,Q大致与状态向量x或测量值y的顺序相同分别。
如果您不喜欢带噪声矩阵的模糊,可以使用递归拟合:RLS(递归最小二乘法)是一种标准识别方法——但它不使用任何统计数据作为卡尔曼滤波器,即它很容易测量中的噪声。它包含更少的代码行,但也使用持久变量。
function [x_est] = RLS(y,x0,mk,fnc)
% (non)linear recursive-least-square
%
% y measurements
% x0 initial value of the to-be-identified state vector
% mk measurement matrix, so that y = mk'*x
% fnc function handle, if the system is non-linear and when mk is the
% linearized version of this function
persistent x P
if isempty(x)
x = x0;
P = eye(length(x)) * 1e3;
end
% adaption factor (usually [0.9 1))
AdaptFct = .995;
%% nonlinear prediction
if nargin > 4
y_sim = fnc(x);
else
y_sim = mk*x;
end
e = y_sim - y;
%% RLS
% Kalman gain
G = P*mk / (AdaptFct + mk'*P*mk);
% Update
% state
x = x + G*e;
% covarianve matrix
I = eye(length(P));
P_new = (I - G*mk')*P;
P = P_new/AdaptFct;
%% output
x_est = x;
end
请注意,如果要重新启动所有内容,则必须清除函数的持久变量:clear RLS