Multiple objects tracking using Extended Kalman Filter Matlab

Question

I want to track multiple objects in 3D space , however I wrote a classdef for visual tracking of an object using Extended Kalman Filter in Matalb . It working is fine for a single object object. However I want to track multiple objects of same 3D space and call this class in external nested for loops. All what I'm misunderstanding/confusing is that, how to call this in external loops to know the predicted value of every object. I have the Constructor in which assumptions and initializing of variables are defined, and in my understanding it must be initialized once for every object, not in every loop iteration.

How to initialize this for every object and get the predicted value. The assumptions/constructor can only be defined outside the class as it assumes the first 2 rows of an object.

Please help me to get out of this, its so confusing for me.

My External Loops:

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

My Extended Kalman Filter:

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

If the code is working fine for a single object, then you could create an array of Kalman filters for each object that you are tracking. That way, the EKF is associated with a single object and not with a group (since the state and covariance matrices, X and P respectively are specific for a single object that has been predicted (and later corrected/updated?) with some observation.

I'm a little unclear on what your double loops are - presumably you just have five objects and 1000 observations for each, so you could do something like the following

% 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

The above will conflict with your code for two reasons - the EKFpredictor is being passed a new position (rather then re-creating a new EKF) and the return value from this function is being re-assigned to the cell array (so that we always maintain the most up-to-date version of the EKF for that object). That means your function signature must change to

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

The new position is being passed because presumably you are using the previous iteration's position and correcting (or updating) it given the new one. The EKF's state and covariance ( X and P ) are to be updated with the new measurement state and covariance (usually denoted by Z and R ). I noticed that you have the Z but not the R (that could be your M ?).

But then in the EKFPredictor method, the code iterates 100 times - why is that?

I don't see any explicit prediction in the EKFPredictor method with a transition matrix F and a time delta between the previous update and the current one. Is that not anything you have to be concerned with, or is it just hidden?

I'm hoping that the above is somewhat helpful though it may be not what you are expecting. But you do have to create separate EKFs for each object. Try it and see what happens.