Question

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

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

请帮助我摆脱这种困扰，这对我来说太困惑了。

我的外部循环：

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。试一试，看看会发生什么。

使用扩展卡尔曼滤波器Matlab进行多个对象跟踪

1 个答案: