基于扩展卡尔曼滤波Matlab的多目标跟踪

Question

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

如何为每个对象初始化此值并获取预测值。假设/构造函数只能在类之外定义，因为它假定对象的前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

如果代码对单个对象运行良好，那么您可以为正在跟踪的每个对象创建一个Kalman滤波器数组。这样，EKF是与单个对象关联的，而不是与组关联的(因为状态矩阵和协方差矩阵、X和P分别是特定于已预测的单个对象(稍后修正/更新？)带着一些观察。

我有点不清楚你的双循环是什么--大概你只有5个对象和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。试试看会发生什么。