Question

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

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

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

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

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

Answer 1

我的建议是转到Mathworks file exchange和search for Kalman filters

您可以找到这个非常标准的算法的几个好的代码片段。

就卡尔曼自我过滤而言，它们就是所谓的预测器 - 估计器。也就是说，他们可以根据时间n的观察结果预测n-1时的状态。然后，当您在n时间收到观察结果后，您可以对所有时间（包括时间n进行估算（有些人称之为平滑）。估算部分通过所谓的创新和当前的卡尔曼增益来完成。

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

你必须选择一个状态转移矩阵，你必须对你的观察误差的协方差矩阵有一些了解，你的状态转移矩阵中的误差的协方差矩阵（即，有多好）您的状态转换模型是），以及您的初始状态估计的协方差矩阵（您还必须选择）。您还必须选择状态向量和观察向量之间的关系。

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

Answer 2

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

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

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

Answer 3

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

Answer 4

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

Answer 5

在您的情况下，（线性）卡尔曼滤波器可能是最好的选择（实际上，它的第一个应用就是跟踪阿波罗太空船的位置以正确击中月球！）。因此，有很多关于此问题的教程，例如请参阅example with this cute little robot。实际上是大约5行代码（请注意，您应该使用 persistent 变量）。协方差矩阵（通常是 P ， R ， Q ）的调整是有根据的猜测。将P初始化为对角矩阵P = eye(length(x))*1e3，并大致以状态向量 x R ， Q >或分别为 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

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

5 个答案: