基于传感器融合的卡尔曼滤波多点定位算法matlab仿真

classdef mlat % to run Mlat you need to use main funciton % > do_main('algorithm', anchor Location, distances, varargin) % > 'algorithms' are four: gradient descent, recursive trilateration, % simple least square, normal trilateration % > varargin: using bounds for gradient descent and weighting matrix (error % variance matrix) for recursive and simple trilateration algorithms % > MLAT is class contains 4 different algorithms for Multilateration and % those are: % 1- Gradient Decent using two funtions: % a- gd() % b- do_gdescent() % 2- Recursive Trilateration using three funtions: % a- RecTrilateration() % b- distancen() % c- lsrec() % d- do_RecTri() % 3- Simple Least Square Error algorithm using one funtion do_LLS() % 4- normal Trilateration algorithm using one funtion do_Tri() methods (Access = public ,Static) %% main function function position = do_main(Algorithm, anchor_Location, ranges_in, varargin) bounds_in = findBounds(anchor_Location); W = eye(length(ranges_in)); if length(varargin) ~= 0 for i = 1:2:length(varargin{1}) switch (varargin{1}{i}) case 'bounds' bounds_in = varargin{1}{i+1}; case 'weight' W = varargin{1}{i+1}; case 'Null' continue; end end end switch Algorithm case 'Gradient_Descent' position = mlat.do_gdescent(anchor_Location, ranges_in, 'bounds',bounds_in); case 'Recursive_Trilateration' position = mlat.do_RecTri(anchor_Location', ranges_in, W); case 'Least_square' position = mlat.do_LLS(anchor_Location', ranges_in); case 'Trilateration' position = mlat.do_Tri(anchor_Location',ranges_in, W); end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% Gradinet Descent Algorithm %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Gradient Descent Algorithm % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function result_table = gd(anchors_in, ranges_in, varargin) % varargin: 'bounds_in', 'n_trial', 'alpha', 'time_threshold' bounds_in = []; n_trial = 1000; alpha = 0.0001; time_threshold = 1/ n_trial; for i = 1:2:length(varargin{1}) switch (varargin{1}{i}) case 'bounds' bounds_in = varargin{1}{i+1}; case 'trial' n_trial = varargin{1}{i+1}; case 'alpha' alpha = varargin{1}{i+1}; case 'time' time_threshold = varargin{1}{i+1}; end end [n, dim] = size(anchors_in); bounds_temp = [anchors_in; bounds_in]; bounds(1, :) = min(bounds_temp); bounds(2, :) = max(bounds_temp); ranges = nan(1, n); result_table = nan(n_trial, dim + 1); for i = 1:n_trial estimator0 = nan(1, dim); % estimator0(1:2) = [1.5 2.2]; % estimator0(3) = (bounds(2, 3) - bounds(1, 3)) * rand + bounds(1, 3); for j = 1:dim estimator0(j) = (bounds(2, j) - bounds(1, j)) * rand + bounds(1, j); end estimator = estimator0; t0 = tic; while true for j = 1:n ranges(j) = norm(anchors_in(j, :) - estimator); end err = norm(ranges_in - ranges); delta = zeros(1, dim); for j = 1:n delta = delta + (ranges_in(j) - ranges(j)) / ranges(j) * (estimator - anchors_in(j, :)); end delta = 2 * alpha * delta; estimator_next = estimator - delta; for j = 1:n ranges(j) = norm(anchors_in(j, :) - estimator_next); end err_next = norm(ranges_in - ranges); if err_next < err estimator = estimator_next; else result_table(i, 1:dim) = estimator; result_table(i, end) = err; break; end if toc - t0 > time_threshold break; end end end end function estimator = do_gdescent(anchors_in, ranges_in, varargin) result_table = mlat.gd(anchors_in, ranges_in, varargin); [~, I] = min(result_table(:, end)); estimator = round(result_table(I, 1:end-1),2); end %% Simple Least Square Error algorithm %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Least Square Error Simple Trilateration % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function Nmat = do_LLS(anchors_in,ranges_in) A = []; b = []; [m n] = size(anchors_in); x = anchors_in(1,:); y = anchors_in(2,:); z = anchors_in(3,:); for i=2:n A(i-1,:) = [x(i)-x(1) , y(i)-y(1) , z(i)-z(1)]; b(i-1,:) = ranges_in(1)^2 - ranges_in(i)^2 + x(i)^2 + y(i)^2 + z(i)^2 - x(1)^2 - y(1)^2 - z(1)^2; end %A = 2*A; Nmat = pinv(A'*A)*A'*b/2; end %% Recursive Trilateration %%%%%%%%%%%%%%%%%%%%%%%%%%% % Recursive Trilateration % %%%%%%%%%%%%%%%%%%%%%%%%%%% function Nmat = RecTrilateration(P,S,W) % paper "An algebraic solution to the multilateration problem" % Author: Norrdine, Abdelmoumen (norrdine@hotmail.de) % https://www.researchgate.net/publication/275027725_An_Algebraic_Solution_to_the_Multilateration_Problem % usage: [N1 N2] = RecTrilateration(P,S,W) % P = [P1 P2 P3 P4 ..] Reference points matrix % S = [s1 s2 s3 s4 ..] distance matrix. % W : Weights Matrix (Statistics). % N : calculated solution [mp,np] = size(P); ns = length(S); if (ns~=np) error('number of reference points and ranges and not the same'); end A=[]; b=[]; for i1=1:np x = P(1,i1); y = P(2,i1); z = P(3,i1); s = S(i1); A = [A ; 1 -2*x -2*y -2*z]; b= [b ; s^2-x^2-y^2-z^2 ]; end if (np==3) warning off; Xp= pinv(A)*b; % Gaussian elimination % or Xp=pinv(A)*b; P % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) % depend only on the reference points % it could be computed only once xp = Xp(2:4,:); Z = null(A); z = Z(2:4,:); if rank (A)==3 %Polynom coeff. a2 = z(1)^2 + z(2)^2 + z(3)^2 ; a1 = 2*(z(1)*xp(1) + z(2)*xp(2) + z(3)*xp(3))-Z(1); a0 = xp(1)^2 + xp(2)^2+ xp(3)^2-Xp(1); p = [a2 a1 a0]; t = roots(p); %solution N1 = Xp + t(1)*Z; N2 = Xp + t(2)*Z; Nmat(:,1) = N1; Nmat(:,2) = N2; end end A0 = A(1:3,:); if (np>3) P10=P(:,1:end-1);S10=S(:,1:end-1);W0=W(1:end-1,1:end-1); N0mat = mlat.RecTrilateration(P10,S10,W0); N01 = N0mat(:,1); N02 = N0mat(:,2); %select N0 C = W'*W; Xpdw = pinv(A'*C*A)*A'*C*b; % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) % depend only on the reference points % it could be computed only once NormErrorXpdw = Xpdw(1)-norm(Xpdw(2:4))^2; if (norm(Xpdw(2:4)-N01(2:4))<norm(Xpdw(2:4)-N02(2:4))) N0 = N01; else N0 = N02; end Nmat(:,1)= N01; Nmat(:,2)= N02; W0 = W(1:3,1:3); C0 = W0*W0'; P_0 = inv(A0'*C0*A0); %Start solution invP_i_1 = inv(P_0);