使用扩展卡尔曼滤波器 Matlab 进行多目标跟踪

Question

我想跟踪 3D 空间中的多个对象，但是我写了一个 classdef 用于在 Matalb扩展卡尔曼滤波器 对对象进行视觉跟踪/em>。它适用于单个对象对象。但是，我想跟踪相同 3D 空间 的多个对象并在外部嵌套 for 循环中调用此类。我所误解/困惑的是，如何在外部循环中调用它来了解每个对象的预测值。我有Constructor，其中定义了变量的假设和初始化，据我所知，它必须为每个对象初始化一次，而不是在每次循环迭代中。

如何为每个对象初始化 this 并获得预测值。假设/构造函数只能在类外部定义，因为它假设对象的前 2 行。

请帮我摆脱困境，这让我很困惑。

我的外部循环：

for ii = i:1000  % position of objects
for jj = 1:5 %5 objects
predictedS = EKF(obj{jj}(ii,:));
predictedS=predictedS.EKFpredictor;
end

我的扩展卡尔曼滤波器：

classdef EKF <handle
    properties (Access=private)
        H
        K
        Z
        Q
        M
        ind
        A
        X
        Xh
        P
        a
        b
    end
    methods
        function obj = EKF(position)
            obj.H = [];
            obj.K = [];
            obj.Z  = [];
            obj.ind=0; % indicator function. Used for unwrapping of tan
            obj.Q =[0 0 0 0 0 0;
                0 0 0 0 0 0;
                0 0 0 0 0 0;
                0 0 0 0.01 0 0;
                0 0 0 0 0.01 0;
                0 0 0 0 0 0.01];% Covarience matrix of process noise
            obj.M=[0.001 0 0;
                0 0.001 0;
                0 0 0.001]; % Covarience matrix of measurment noise
            obj.A=[1 0 0 0.1 0 0;
                0 1 0 0 0.1 0;
                0 0 1 0 0 0.1;
                0 0 0 1 0 0;
                0 0 0 0 1 0;
                0 0 0 0 0 1]; % System Dynamics
            obj.X(:,1)=position(1:6,1); % Actual initial conditions
            obj.Z(:,:,1)=position(1,:)';% initial observation
            obj.Xh(:,1)=position(1:6,1);%Assumed initial conditions
            obj.P(:,:,1)=[0.1 0 0 0 0 0;
                0 0.1 0 0 0 0;
                0 0 0.1 0 0 0;
                0 0 0 0.1 0 0;
                0 0 0 0 0.1 0;
                0 0 0 0 0 0.1]; %inital value of covarience of estimation error

        end

        function predictedS=EKFpredictor(obj)
            function   [ARG]=arctang(a,b,ind)
                if b<0 && a>0 % PLACING IN THE RIGHT QUADRANT
                    ARG=abs(atan(a/b))+pi/2;
                elseif b<0 && a<0
                    ARG=abs(atan(a/b))+pi;
                elseif b>0 && a<0
                    ARG=abs(atan(a/b))+3*pi/2;
                else
                    ARG=atan(a/b);
                end
                if ind==-1 % UNWARPPING PART
                    ARG=ARG-2*pi;
                else
                    if ind==1;
                        ARG=ARG+2*pi;
                    end
                end
            end

            for n = 1:100
            obj.X(:,n+1)=obj.A*obj.X(:,n)+[0;0;0;sqrt(obj.Q(4,4))*randn(1);sqrt(obj.Q(5,5))*randn(1);sqrt(obj.Q(6,6))*randn(1)];
            obj.Z(:,n+1)=[sqrt(obj.X(1,n)^2+obj.X(2,n)^2);arctang(obj.X(2,n),obj.X(1,n),obj.ind);obj.X(3,n)]+[sqrt(obj.M(1,1))*randn(1);sqrt(obj.M(1,1))*randn(1);sqrt(obj.M(1,1))*randn(1)];
            obj.Xh(:,n+1)=obj.A*obj.Xh(:,n);
            predictedS=obj.Xh';
            obj.P(:,:,n+1)=obj.A*obj.P(:,:,n)*obj.A'+obj.Q;
            obj.H(:,:,n+1)=[obj.Xh(1,n+1)/(sqrt(obj.Xh(1,n+1)^2+obj.Xh(2,n+1)^2)), obj.Xh(2,n+1)/(sqrt(obj.Xh(1,n+1)^2+obj.Xh(2,n+1)^2)),0,0,0,0; ...
                -obj.Xh(2,n+1)/(sqrt(obj.Xh(1,n+1)^2+obj.Xh(2,n+1)^2)), obj.Xh(1,n+1)/(sqrt(obj.Xh(1,n+1)^2+obj.Xh(2,n+1)^2)),0,0,0,0; ...
                0,0,1,0,0,0];
            obj.K(:,:,n+1)=obj.P(:,:,n+1)*obj.H(:,:,n+1)'*(obj.M+obj.H(:,:,n+1)*obj.P(:,:,n+1)*obj.H(:,:,n+1)')^(-1);
            Inov=obj.Z(:,n+1)-[sqrt(obj.Xh(1,n+1)^2+obj.Xh(2,n+1)^2);arctang(obj.Xh(2,n+1),obj.Xh(1,n+1),obj.ind);obj.Xh(3,n+1)];
            obj.Xh(:,n+1)=obj.Xh(:,n+1)+ obj.K(:,:,n+1)*Inov;
            obj.P(:,:,n+1)=(eye(6)-obj.K(:,:,n+1)*obj.H(:,:,n+1))*obj.P(:,:,n+1);
            theta1=arctang(obj.Xh(1,n+1),obj.Xh(2,n+1),0);
            theta=arctang(obj.Xh(1,n),obj.Xh(2,n),0);
            if abs(theta1-theta)>=pi
                if obj.ind==1
                    obj.ind=0;
                else
                    obj.ind=1;
                end
            end
            end
        end

    end
end

end

Answer 1

如果代码对单个对象运行良好，那么您可以为要跟踪的每个对象创建一组卡尔曼滤波器。这样，EKF 与单个对象而不是与组相关联（因为状态和协方差矩阵，X 和 P 分别特定于已预测的单个对象（后来更正/更新？）有一些观察。

我有点不清楚您的双循环是什么 - 大概您只有五个对象和每个对象 1000 个观察值，因此您可以执行以下操作

% there are 5 objects
numObjs = 5;

% initialize a cell array of EKFs
ekfs = cell(numObjs,1);

% initialize the EKF (tracker) for each object given the first observation
for i=1:numObjs
    ekfs{i} = EKF(obj{i}(1,:));  % so get the first obs for object i
end

% now predict and correct each object with the new observation (new position of object)
for j=2:1000
    for i=1:numObjs
        ekfs{i} = ekfs{i}.EKFpredictor(obj{i}(j,:));
    end
end

以上内容与您的代码冲突有两个原因 - EKFpredictor 被传递一个新位置（而不是重新创建一个新的 EKF），并且该函数的返回值被重新分配给元胞数组（以便我们始终为该对象维护最新版本的 EKF）。这意味着您的函数签名必须更改为

function [obj,predictedS]=EKFpredictor(obj,position)

正在传递新位置，因为大概您正在使用上一次迭代的位置并在给定新位置的情况下更正（或更新）它。 EKF 的状态和协方差（X 和 P）将使用新的测量状态和协方差（通常表示为 Z 和 R）进行更新。我注意到你有Z，但没有R（那可能是你的M？）。

但是在 EKFPredictor 方法中，代码迭代了 100 次 - 为什么会这样？

我在EKFPredictor 方法中没有看到任何明确的预测，其中包含一个转换矩阵F 以及上一次更新和当前更新之间的时间差。这不是你必须关心的事情，还是只是隐藏起来？

我希望以上内容有所帮助，尽管它可能不是您所期望的。但是您必须为每个对象创建单独的 EKF。试试看会发生什么。