卡尔曼滤波器，汽车跟踪，Matlab

我有一个移动汽车的小数据集：

Data_=[time x,y,z];    %# ONLY THIS DATA

我知道在这种情况下，速度和加速度不是恒定的。

我想估计不同时间的汽车位置。我决定使用卡尔曼滤波器。我搜索了卡尔曼滤波器，但找不到在 3D 空间中以速度和加速度跟踪对象的代码。我不知道从哪里开始。卡尔曼滤波器可以自动处理速度和加速度吗？

有人可以帮助我并提供一些链接或一些指导吗？

我的建议是去Mathworks文件交换并搜索卡尔曼滤波器

你会为这个非常标准的算法找到几段很好的代码。

就卡尔曼滤波本身而言，它们就是所谓的预测-估计器。也就是说，他们可以在给定截至n-1的观测值的情况下n对状态进行预测。然后，在收到时间n的观测值后，您可以对所有时间进行估计（有人称之为平滑），包括时间n。估算部分是通过所谓的创新和当前的卡尔曼收益来完成的。

卡尔曼滤波器通过"状态空间"的概念工作，即你的状态存储有关对象的所有必要信息。 不同的观察向量是您可以观察到的关于系统的。例如，在恒定加速度模型中，您可能会假设状态仅包含 3 个位置值和 3 个速度值（每个值为 x、y 和 z）。过滤器工作的设计者决定状态空间和状态转换模型（在没有观察的情况下，您期望状态如何变化。

你必须选择一个状态转移矩阵，你必须对观测误差的协方差矩阵、状态转移矩阵中误差的协方差矩阵（

即状态转移模型的好坏）和初始状态估计的协方差矩阵（你也必须选择）有一定的了解。您还必须选择状态向量和观察向量之间的关系。

卡尔曼滤波器是最大似然最优线性估计器，如果你假设高斯观测噪声、高斯过程噪声和其他一些标准的东西。

Kalman Filter循环中有 5-6 行。你不需要任何人的实现。

您需要的是一个描述汽车轨迹的线性系统模型。如果你有系统矩阵A,B,C（或F,G,H），你实际上就完成了。

Kalman Filter是一种通用的Bayesian过滤算法。它适用于任何线性高斯情况。

查看 AI 的 udacity.com 课程，CS373。在那里，他们很好地解释了卡尔曼滤波器。

计算机视觉系统工具箱现在有一个vision.KalmanFilter对象。下面是如何使用它来跟踪对象的示例。该示例是 2D 的，但可以很容易地推广到 3D。

（线性）卡尔曼滤波器可能是您最好的选择（它的第一个应用实际上是跟踪阿波罗宇宙飞船的位置以正确撞击月球！所以有很多关于这个问题的教程，例如，看看这个可爱的小机器人的例子。它实际上是大约 5 行代码（请注意，您应该使用持久变量）。协方差矩阵（通常为 P、R、Q）的调整是一种有根据的猜测。将 P 初始化为对角矩阵 P = eye(length(x))*1e3，并分别按照状态向量 x 或测量 y 的大致相同顺序选择噪声矩阵 R、Q。

如果你不喜欢带有噪声矩阵的模糊，你可以使用递归拟合：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

相关内容

最新更新

热门标签：