extanded kalman filter for tdoa 3D positioning

Question

I am trying to use the Extended Kalman filter to locate my mobile device in the spice using the TDoA method.
The problem I get that is my cone doesn't converge to the ground truth position.
Sometimes the matrix P, Xh, and X have complex values is that OK?
Have I to modify something or to add stop conditions.
a part of my code is like

%% inititialisation
% Covarience matrix of process noise
Q=[ 0.01 0 0;0 0.01 0;0 0 0.01];
% Covarience matrix of measurment noise
R=[ 0.001  0.0005 0.0005 ; 0.0005 0.001  0.0005 ; 0.0005 0.0005 0.001];
% System Dynamics
A=[1 0 0 ;0 1 0 ;0 0 1 ];
%Assumed initial conditions
Xh(:,1)=[0 0 0]';
B1=[2 2 3; 1.5 2 3; 2.5 2 3; 2 1.5 3; 2 2.5 3]; %the position of 5 beacons installes in the ceiling
%inital value of covarience of estimation error
P(:,:,1)=[0 0 0 ; 0 0 0 ; 0 0 0 ];
Xb=[2 2 2];
X(:,1)=Xb;%
Z(:,1)=Dd20(:,1);%Dd20 is a matrix of TDOA 3*n ofsize
for n=1:2000
% PROCESS AND OBSERVATION PROCESS WITH GAUSSINA NOISE
X(:,n+1)=A*X(:,n)+[sqrt(((Q(1,1))*randn(1)));sqrt((Q(2,2))*randn(1));sqrt((Q(3,3))*randn(1))]; % State process % w generating process noise

Z(:,n+1)=Z(:,n)+[sqrt(R(1,1))*randn(1);sqrt(R(1,1))*randn(1);sqrt(R(1,1))*randn(1)];%mesurment matrix

hsn(:,n+1)=[(sqrt(((X(1,n+1)-B1(5,1))^2+(X(2,n+1)-B1(5,2))^2+(X(3,n+1)-B1(5,3))^2))-(sqrt(((X(1,n+1)-B1(1,1))^2+(X(2,n+1)-B1(2,1))^2+(X(3,n+1)-B1(1,3))^2))));
(sqrt(((X(1,n+1)-B1(2,1))^2+(X(2,n+1)-B1(2,2))^2+(X(3,n+1)-B1(2,3))^2))-(sqrt(((X(1,n+1)-B1(1,1))^2+(X(2,n+1)-B1(2,1))^2+(X(3,n+1)-B1(1,3))^2))));
(sqrt(((X(1,n+1)-B1(4,1))^2+(X(2,n+1)-B1(4,2))^2+(X(3,n+1)-B1(4,3))^2))-(sqrt(((X(1,n+1)-B1(1,1))^2+(X(2,n+1)-B1(2,1))^2+(X(3,n+1)-B1(1,3))^2))))]  +[sqrt(((R(1,1))*randn(1)));sqrt(((R(1,1))*randn(1)));sqrt(((R(1,1))*randn(1)))];

% prediction of next state
Xh(:,n+1)=A*Xh(:,n);% ESTIMATE
P(:,:,n+1)=A*P(:,:,n)*A'+Q;% PRIORY ERROR COVARIENCE

% CORRECTION EQUTIONS
H(:,:,n+1)=[(Xh(1,n+1)-B1(5,1))/((sqrt(((Xh(1,n+1)-B1(5,1)^2)+(Xh(2,n+1)-B1(5,2)^2)+(Xh(3,n+1)-B1(5,3)^2))))-(Xh(1,n+1)-B1(5,1))/(sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),...
(Xh(2,n+1)-B1(5,2))/((sqrt(((Xh(1,n+1)-B1(5,1)^2)+(Xh(2,n+1)-B1(5,2)^2)+(Xh(3,n+1)-B1(5,3)^2))))-(Xh(2,n+1)-B1(5,2))/(sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(3,n+1)-B1(5,3))/((sqrt(((Xh(1,n+1)-B1(5,1)^2)+(Xh(2,n+1)-B1(5,2)^2)+(Xh(3,n+1)-B1(5,3)^2))))-(Xh(3,n+1)-B1(5,3))/(sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2)))));
(Xh(1,n+1)-B1(2,1))/(sqrt(((Xh(1,n+1)-B1(2,1)^2)+(Xh(2,n+1)-B1(2,2)^2)+(Xh(3,n+1)-B1(2,3)^2))))-(Xh(1,n+1)-B1(1,1))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(2,n+1)-B1(2,2))/(sqrt(((Xh(1,n+1)-B1(2,1)^2)+(Xh(2,n+1)-B1(2,2)^2)+(Xh(3,n+1)-B1(2,3)^2))))-(Xh(2,n+1)-B1(1,2))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(3,n+1)-B1(2,3))/(sqrt(((Xh(1,n+1)-B1(2,1)^2)+(Xh(2,n+1)-B1(2,2)^2)+(Xh(3,n+1)-B1(2,3)^2))))-(Xh(3,n+1)-B1(1,3))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2)))));
(Xh(1,n+1)-B1(4,1))/(sqrt(((Xh(1,n+1)-B1(4,1)^2)+(Xh(2,n+1)-B1(4,2)^2)+(Xh(3,n+1)-B1(4,3)^2))))-(Xh(1,n+1)-B1(1,1))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(2,n+1)-B1(4,2))/(sqrt(((Xh(1,n+1)-B1(4,1)^2)+(Xh(2,n+1)-B1(4,2)^2)+(Xh(3,n+1)-B1(4,3)^2))))-(Xh(2,n+1)-B1(1,2))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2))))),
(Xh(3,n+1)-B1(4,3))/(sqrt(((Xh(1,n+1)-B1(4,1)^2)+(Xh(2,n+1)-B1(4,2)^2)+(Xh(3,n+1)-B1(4,3)^2))))-(Xh(3,n+1)-B1(1,3))/((sqrt(((Xh(1,n+1)-B1(1,1)^2)+(Xh(2,n+1)-B1(1,2)^2)+(Xh(3,n+1)-B1(1,3)^2)))));
 ]; % Jacobian matrix

%THIS SUBROTINE COMPUTES KALMAN GAIN
K(:,:,n+1)=P(:,:,n+1)*H(:,:,n+1)'*(R+H(:,:,n+1)*P(:,:,n+1)*H(:,:,n+1)')^(-1);
Inov=Z(:,n+1)-hsn(:,n);% INNOVATION
Xh(:,n+1)=Xh(:,n+1)+ K(:,:,n+1)*Inov; %computes final estimate
P(:,:,n+1)=(eye(3)-K(:,:,n+1)*H(:,:,n+1))*P(:,:,n+1);% %computes covarience of estimation error
end

Unfortunately I cannot execute your code, because some pieces are missing. But to avoid complex numbers you need to find another way of generating noise. You use ´randn(1)´ and it sometimes generates negative numbers. Square root comes to complex numbers in this case. — Anton, Jul 02 '19 at 06:44
Thank you for your answer and help, yes the probleñ was in the negative values of the square roots. So in my case I changed the equations, instead of writing: X(1,n+1)-B1(5,1))^2 I write: B1(5,1)-X(1,n+1))^2. So it is working well now. — khaoula mannay, Jul 04 '19 at 09:38

extanded kalman filter for tdoa 3D positioning

0 Answers0