Отслеживание нескольких объектов с использованием расширенного фильтра Калмана Matlab

Я хочу отслеживать несколько объектов в трехмерном пространстве, однако я написал classdef для визуального отслеживания объекта с помощью расширенного фильтра Калмана в Matalb. Он работает нормально для одного объекта объекта. Однако я хочу отслеживать несколько объектов одного и того же трехмерного пространства и вызывать этот класс во внешних вложенных циклах 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

object matlab kalman-filter
person user3593525    schedule 16.06.2014    source источник

Ответы (1)


arrow_upward
1
arrow_downward

Если код работает нормально для одного объекта, вы можете создать массив фильтров Калмана для каждого объекта, который вы отслеживаете. Таким образом, 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 для каждого объекта. Попробуйте и посмотрите, что произойдет.

person Geoff    schedule 16.06.2014
comment
У меня есть вложенные циклы, в которых внешний цикл повторяет положение объекта, а внутренний - для повторения объектов, т.е. для каждого значения позиции каждый объект должен быть в действии. И да, M — это моя ковариационная матрица измерительного шума. Я создал только для одного объекта, поэтому я использовал цикл for из 100, но теперь это нужно исключить, и как я могу ввести n по отношению к моему внешнему циклу? - person user3593525; 17.06.2014
comment
Мне до сих пор неясно, что такое n - это количество наблюдений за объектом? Если да, то почему внешний цикл повторяется 1000 раз? Кстати, какова ваша матрица состояния X - это 2D-позиция и скорость, 3D и т. д.? Каковы ваши наблюдения? Другая 2D или 3D позиция? - person Geoff; 17.06.2014
comment
Извините за неясность, n - это количество наблюдений, такое же, как и во внешнем цикле ii. Я хочу ввести наблюдение ii вместо n, взяв прогноз, а затем снова ввести предыдущую позицию. должен быть n=ii и исключает цикл for n. Только трехмерное положение. - person user3593525; 17.06.2014
comment
function [obj,predictedS]=EKFpredictor(obj,position) в сигнатуре функции, я хочу дать ii и position(ii) и вывести то же самое. - person user3593525; 17.06.2014
comment
Круто, что n совпадает с внешним циклом. Вам не нужно передавать как ii, так и position(ii), если вы не используете индекс для отслеживания состояния и ковариации при каждом обновлении (история). - person Geoff; 17.06.2014
comment
Я не понял вашу мысль, как передать и обновить позицию в каждой точке? пожалуйста, уточните это относительно моего кода. - person user3593525; 17.06.2014
comment
На каждой итерации внешнего цикла теперь вы будете передавать только наблюдение (новую 3D-позицию) для объекта, который будет использоваться для обновления текущих X и P объекта. Если вы все еще хотите создать экземпляр объекта со ВСЕМИ 100 (или 1000) позициями? - person Geoff; 17.06.2014
comment
Вы хотите сказать, что моя подпись functino должна быть такой function [obj,predictedS]=EKFpredictor(obj,position(ii,:)) и как насчет n ? - person user3593525; 17.06.2014
comment
Нет - сигнатура функции остается как [obj,predictedS]=EKFpredictor(obj,position), но когда вы вызываете ее из своего цикла, вам понадобится что-то вроде [ekfs{i},predictedS] = ekfs{i}.EKFpredictor(objectArray{i}(j,:));, где вы передаете ТОЛЬКО новую позицию (или, тем не менее, вы получаете доступ к этой единственной позиции на j-й итерации цикла for - я не знаю, как выглядит эта структура). Обратите внимание, что я переименовал массив ячеек из пяти объектов (с их наблюдениями) в objectArray, чтобы было ясно, что это отличается от obj, используемого в вашем определении класса. - person Geoff; 17.06.2014
comment
Давайте продолжим обсуждение в чате. - person user3593525; 17.06.2014