├── PathOption2.mat ├── PathOption3.mat ├── PathOption5.mat ├── pathOption1.mat ├── pathOption4.mat ├── Rotation ├── plot_angles.m ├── rotationMat.m ├── angles.m └── main_angles.m ├── UltraSonic_study ├── input.mat ├── input2.mat ├── errorFun.m ├── ErrorCalculation.m ├── findBounds.m ├── ekf.m ├── errorMeasuresMatrix.m ├── colldetect.m ├── cubeData.m ├── definedFigure2.m ├── plot_data2.m ├── preDefinedMatrix2.m ├── main2.m └── mlat.m ├── errorFun.m ├── ekf.m ├── colldetect.m ├── cubeData.m ├── README.md ├── ErrorCalculation └── ErrorCalculation.m ├── LICENSE ├── definedFigure2.m ├── plot_data2.m ├── preDefinedMatrix.m ├── main.m └── mlat.m /PathOption2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/PathOption2.mat -------------------------------------------------------------------------------- /PathOption3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/PathOption3.mat -------------------------------------------------------------------------------- /PathOption5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/PathOption5.mat -------------------------------------------------------------------------------- /pathOption1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/pathOption1.mat -------------------------------------------------------------------------------- /pathOption4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/pathOption4.mat -------------------------------------------------------------------------------- /Rotation/plot_angles.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/Rotation/plot_angles.m -------------------------------------------------------------------------------- /UltraSonic_study/input.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/UltraSonic_study/input.mat -------------------------------------------------------------------------------- /UltraSonic_study/input2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mghojal/Localization-Algorithm/HEAD/UltraSonic_study/input2.mat -------------------------------------------------------------------------------- /UltraSonic_study/errorFun.m: -------------------------------------------------------------------------------- 1 | function [errorMLat errorMLatKF] = errorFun(posReal,F,i,k) 2 | errorMLat = sqrt((posReal(k,1)-F(i,1))^2+(posReal(k,2)-F(i,2))^2+(posReal(k,3)-F(i,3))^2); 3 | errorMLatKF = sqrt((posReal(k,1)-F(i,4))^2+(posReal(k,2)-F(i,5))^2+(posReal(k,3)-F(i,6))^2); -------------------------------------------------------------------------------- /errorFun.m: -------------------------------------------------------------------------------- 1 | % calculating mean error in 3D space 2 | function [errorMLat errorMLatKF] = errorFun(posReal,F,i,k) 3 | errorMLat = sqrt((posReal(k,1)-F(i,1))^2+(posReal(k,2)-F(i,2))^2+(posReal(k,3)-F(i,3))^2); 4 | errorMLatKF = sqrt((posReal(k,1)-F(i,4))^2+(posReal(k,2)-F(i,5))^2+(posReal(k,3)-F(i,6))^2); 5 | end -------------------------------------------------------------------------------- /UltraSonic_study/ErrorCalculation.m: -------------------------------------------------------------------------------- 1 | function ErrorM = ErrorCalculation(posEst, posReal, AnNo) 2 | for i=1:length(posEst) 3 | ErrorM(i,1) = posEst(i,1) - posReal(i,1); 4 | ErrorM(i,2) = posEst(i,2) - posReal(i,2); 5 | ErrorM(i,3) = posEst(i,3) - posReal(i,3); 6 | end 7 | end -------------------------------------------------------------------------------- /UltraSonic_study/findBounds.m: -------------------------------------------------------------------------------- 1 | function [bounds] = findBounds(anchorLoc) 2 | for i = 1:size(anchorLoc, 2) 3 | bounds(1, i) = min(anchorLoc(:, i)); % minimum boundary of ith axis 4 | bounds(2, i) = max(anchorLoc(:, i)); % maximum boundary of ith axis 5 | end 6 | % hard coded minimum height (0 m) of search boundary 7 | bounds(1, end) = 0; -------------------------------------------------------------------------------- /ekf.m: -------------------------------------------------------------------------------- 1 | function [X, P] = ekf(A,C,Q,H,R,x_t,Y_t,pHat) 2 | %dt: time second 3 | %x_t previous predicted value 4 | %Y_t: measured value 5 | %X state estimated value 6 | %P covariance estimated state 7 | 8 | 9 | xHat = A*x_t; 10 | pHat = A*pHat*A' + Q; 11 | KG = (pHat*H)/((H*pHat*H') + R); 12 | Y = C*Y_t; 13 | X = xHat + KG*(Y-H*xHat); 14 | P = (eye(9)-KG*H) * pHat; 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /UltraSonic_study/ekf.m: -------------------------------------------------------------------------------- 1 | function [X, P] = ekf(A,C,Q,H,R,x_t,Y_t,pHat) 2 | %dt: time second 3 | %x_t previous predicted value 4 | %Y_t: measured value 5 | %X state estimated value 6 | %P covariance estimated state 7 | 8 | 9 | xHat = A*x_t; 10 | pHat = A*pHat*A' + Q; 11 | KG = (pHat*H)/((H*pHat*H') + R); 12 | Y = C*Y_t; 13 | X = xHat + KG*(Y-H*xHat); 14 | P = (eye(9)-KG*H) * pHat; 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /UltraSonic_study/errorMeasuresMatrix.m: -------------------------------------------------------------------------------- 1 | function [C] = errorMeasuresMatrix(alpha) 2 | 3 | % Measurement matrix 4 | % The expected measurement given the predicted state 5 | C = [1 0 0 0 0 0 0; 6 | 0 1 0 0 0 0 0; 7 | 0 0 alpha 1-alpha 0 0 0; 8 | 0 0 0 0 0 0 0 9 | 0 0 0 0 0 0 0 10 | 0 0 0 0 0 0 0 11 | 0 0 0 0 1 0 0; 12 | 0 0 0 0 0 1 0; 13 | 0 0 0 0 0 0 1]; 14 | 15 | end 16 | -------------------------------------------------------------------------------- /UltraSonic_study/colldetect.m: -------------------------------------------------------------------------------- 1 | function collisionFlag = colldetect(posEst,posObj,SDposEst,SDposObj) 2 | A1 = posEst+3*SDposEst; 3 | A2 = posEst-3*SDposEst; 4 | B1 = posObj+3*SDposObj; 5 | B2 = posObj-3*SDposObj; 6 | if((A1(1) >= B2(1)) && (A2(1) <= B1(1))) 7 | if((A1(2) >= B2(2)) && (A2(2) <= B1(2))) 8 | if((A1(3) >= B2(3)) && (A2(3) <= B1(3))) 9 | collisionFlag = 1; 10 | else 11 | collisionFlag = 0; 12 | end 13 | else 14 | collisionFlag = 0; 15 | end 16 | else 17 | collisionFlag = 0; 18 | end 19 | end -------------------------------------------------------------------------------- /colldetect.m: -------------------------------------------------------------------------------- 1 | function collisionFlag = colldetect(posEst,posObj,MposEst,SDposEst,SDposObj) 2 | A1 = posEst+MposEst+3*SDposEst; 3 | A2 = posEst-MposEst-3*SDposEst; 4 | B1 = posObj+3*SDposObj; 5 | B2 = posObj-3*SDposObj; 6 | if((A1(1) >= B2(1)) && (A2(1) <= B1(1))) 7 | if((A1(2) >= B2(2)) && (A2(2) <= B1(2))) 8 | if((A1(3) >= B2(3)) && (A2(3) <= B1(3))) 9 | collisionFlag = 1; 10 | else 11 | collisionFlag = 0; 12 | end 13 | else 14 | collisionFlag = 0; 15 | end 16 | else 17 | collisionFlag = 0; 18 | end 19 | end -------------------------------------------------------------------------------- /cubeData.m: -------------------------------------------------------------------------------- 1 | function [V Fa] = cubeData(F,MeanErrorMX,MeanErrorMY,MeanErrorMZ,SDErrorMX,SDErrorMY,SDErrorMZ) 2 | Xm = F(1,4)-MeanErrorMX-3*SDErrorMX; 3 | Xp = F(1,4)+MeanErrorMX+3*SDErrorMX; 4 | Ym = F(1,5)-MeanErrorMY-3*SDErrorMY; 5 | Yp = F(1,5)+MeanErrorMY+3*SDErrorMY; 6 | Zm = F(1,6)-MeanErrorMZ-3*SDErrorMZ; 7 | Zp = F(1,6)+MeanErrorMZ+3*SDErrorMZ; 8 | p1 = [Xm Ym Zm]; 9 | p2 = [Xm Yp Zm]; 10 | p3 = [Xp Yp Zm]; 11 | p4 = [Xp Ym Zm]; 12 | p5 = [Xm Ym Zp]; 13 | p6 = [Xm Yp Zp]; 14 | p7 = [Xp Yp Zp]; 15 | p8 = [Xp Ym Zp]; 16 | 17 | V = [p1;p2;p3;p4;... 18 | p2;p3;p7;p6;... 19 | p3;p4;p8;p7;... 20 | p4;p1;p5;p8;... 21 | p1;p2;p6;p5;... 22 | p5;p6;p7;p8]; 23 | Fa = [1 2 3 4; 24 | 5 6 7 8; 25 | 9 10 11 12; 26 | 13 14 15 16; 27 | 17 18 19 20; 28 | 21 22 23 24]; -------------------------------------------------------------------------------- /UltraSonic_study/cubeData.m: -------------------------------------------------------------------------------- 1 | function [V Fa] = cubeData(F,MeanErrorMX,MeanErrorMY,MeanErrorMZ,SDErrorMX,SDErrorMY,SDErrorMZ) 2 | Xm = F(1,4)-MeanErrorMX - 3*SDErrorMX; 3 | Xp = F(1,4)-MeanErrorMX+3*SDErrorMX; 4 | Ym = F(1,5)-MeanErrorMY-3*SDErrorMY; 5 | Yp = F(1,5)-MeanErrorMY+3*SDErrorMY; 6 | Zm = F(1,6)-MeanErrorMZ-3*SDErrorMZ; 7 | Zp = F(1,6)-MeanErrorMZ+3*SDErrorMZ; 8 | p1 = [Xm Ym Zm]; 9 | p2 = [Xm Yp Zm]; 10 | p3 = [Xp Yp Zm]; 11 | p4 = [Xp Ym Zm]; 12 | p5 = [Xm Ym Zp]; 13 | p6 = [Xm Yp Zp]; 14 | p7 = [Xp Yp Zp]; 15 | p8 = [Xp Ym Zp]; 16 | 17 | V = [p1;p2;p3;p4;... 18 | p2;p3;p7;p6;... 19 | p3;p4;p8;p7;... 20 | p4;p1;p5;p8;... 21 | p1;p2;p6;p5;... 22 | p5;p6;p7;p8]; 23 | Fa = [1 2 3 4; 24 | 5 6 7 8; 25 | 9 10 11 12; 26 | 13 14 15 16; 27 | 17 18 19 20; 28 | 21 22 23 24]; -------------------------------------------------------------------------------- /Rotation/rotationMat.m: -------------------------------------------------------------------------------- 1 | function [Rx Ry Rz R] = rotationMat(angle) 2 | phi = angle(1); 3 | theta = angle(2); 4 | psi= angle(3); 5 | 6 | Rx = [ 1 , 0 , 0 ; 7 | 0 , cos(phi) ,-sin(phi); 8 | 0 , sin(phi) , cos(phi)]; 9 | 10 | 11 | Ry = [cos(theta) , 0 , sin(theta); 12 | 0 , 1 , 0 ; 13 | -sin(theta) , 0 , cos(theta) ]; 14 | 15 | 16 | Rz = [cos(psi) , -sin(psi) , 0 ; 17 | sin(psi) , cos(psi) , 0 ; 18 | 0 , 0 , 1 ]; 19 | 20 | R = [cos(angle(2))*cos(angle(3)) sin(angle(1))*sin(angle(2))*cos(angle(3))-cos(angle(1))*sin(angle(3)) cos(angle(1))*sin(angle(2))*cos(angle(3))+sin(angle(1))*sin(angle(3)); 21 | cos(angle(2))*sin(angle(3)) sin(angle(1))*sin(angle(2))*sin(angle(3))+cos(angle(1))*cos(angle(3)) cos(angle(1))*sin(angle(2))*sin(angle(3))-sin(angle(1))*cos(angle(3)); 22 | -sin(angle(2)) sin(angle(1))*cos(angle(2)) cos(angle(1))*cos(angle(2)) ]; 23 | 24 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Localization-Algorithm 2 | Sensor fusion (UWB+IMU+Ultrasonic), using Kalman filter and 3 different multilateration algorithms (Least square and Recursive Least square and gradient descent) 3 | 4 | > in this study we take readings from IMU( accelerometer and gyroscope) calculate linear acceleration to be an input to kalamn filter 5 | 6 | > we take UWB distances reading feed it to multilateration algorithm (there is a choice of three: Least Square, Recurisve Least Square, Gradient Descent) to get first estimated position as an input for Kalman filter 7 | 8 | > we feed also ultrasonic to our Kalman filter 9 | 10 | 11 | > Main file is the main which we can specify all our options on it in addition it refere to the test paths 12 | 13 | > We need to specify the path and the used multilateration algorithm for UWB distances reads 14 | 15 | > In cas we choose gradient descent, we need to specify if we will consider boundaries as our whole working space or minizing it by shrink the boudaries using the previous step. 16 | 17 | Speical study done for horizontal movement which can be found in folder /Ultrasonic_study 18 | -------------------------------------------------------------------------------- /ErrorCalculation/ErrorCalculation.m: -------------------------------------------------------------------------------- 1 | function ErrorM = ErrorCalculation(posEst, posReal, AnNo) 2 | j=1; 3 | for i=1:length(posEst) 4 | if i==1 5 | ErrorM(i,1) = abs(posEst(i,1) - posReal(j,1)); 6 | ErrorM(i,2) = abs(posEst(i,2) - posReal(j,2)); 7 | ErrorM(i,3) = abs(posEst(i,3) - posReal(j,3)); 8 | j=j+1; 9 | elseif AnNo == 5 && rem(i-1,5)==0 || rem(i-1,5)==2 10 | ErrorM(i,1) = abs(posEst(i,1) - posReal(j,1)); 11 | ErrorM(i,2) = abs(posEst(i,2) - posReal(j,2)); 12 | ErrorM(i,3) = abs(posEst(i,3) - posReal(j,3)); 13 | j=j+1; 14 | elseif AnNo == 5 15 | ErrorM(i,1) = abs(posEst(i,1) - posReal(j-1,1)); 16 | ErrorM(i,2) = abs(posEst(i,2) - posReal(j-1,2)); 17 | ErrorM(i,3) = abs(posEst(i,3) - posReal(j-1,3)); 18 | else 19 | ErrorM(i,1) = abs(posEst(i,1) - posReal(j,1)); 20 | ErrorM(i,2) = abs(posEst(i,2) - posReal(j,2)); 21 | ErrorM(i,3) = abs(posEst(i,3) - posReal(j,3)); 22 | j=j+1; 23 | end 24 | end 25 | 26 | end 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 mghojal 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Rotation/angles.m: -------------------------------------------------------------------------------- 1 | function [filtered_angles, gyro_angle, acc_angles] = angles(acc,gyro,dt,pre_gyro_angle) 2 | alpha = 0.95; 3 | %acc angles 4 | phi = atan2(acc(2) , acc(3)); 5 | theta = atan2(acc(3) , acc(1)); 6 | psi = atan2(acc(2) , acc(1)); 7 | % phi = atan(acc(2)/(sqrt(acc(1)^2 + acc(3)^2))); 8 | % theta = -atan(acc(1)/(sqrt(acc(2)^2 + acc(3)^2))); 9 | % psi = atan(sqrt(acc(2)^2 + acc(1)^2)/acc(3)); 10 | acc_angles = [phi theta psi]; 11 | gyro_angle = (gyro*dt)+pre_gyro_angle; 12 | for (k=1:3) 13 | if gyro_angle(k) > pi 14 | gyro_angle(k) = gyro_angle(k) - 2*pi; 15 | elseif gyro_angle(k) <= -pi 16 | gyro_angle(k) = gyro_angle(k) + 2*pi; 17 | else 18 | gyro_angle(k) = gyro_angle(k); 19 | end 20 | end 21 | 22 | for (k=1:3) 23 | if acc_angles(k) > pi 24 | acc_angles(k) = acc_angles(k) - 2*pi; 25 | elseif acc_angles(k) <= -pi 26 | acc_angles(k) = acc_angles(k) + 2*pi; 27 | else 28 | acc_angles(k) = acc_angles(k); 29 | end 30 | end 31 | filtered_angles = [(alpha*gyro_angle(1:2))+((1-alpha)*acc_angles(1:2)), gyro_angle(3)]; 32 | 33 | -------------------------------------------------------------------------------- /definedFigure2.m: -------------------------------------------------------------------------------- 1 | function [aa,ab] = definedFigure2(anchorLoc,figureNumber,MultilaterationAlgorithmName) 2 | 3 | figure('Name',MultilaterationAlgorithmName); 4 | aa=subplot(2,1,1); 5 | plot3(aa,anchorLoc(:,1),anchorLoc(:,2),anchorLoc(:,3),'ko','MarkerSize',8,'lineWidth',2,'MarkerFaceColor','k'); 6 | grid on 7 | hold on 8 | curve = animatedline('Color','r','Marker','o'); 9 | axis([-1 4 -1 4.9 0 3]); 10 | ax = gca; 11 | ax.XTick = (0:0.3:3); 12 | ax.YTick = (0:0.3:3.9); 13 | ax.ZTick = (0:1:3); 14 | xlabel('x axis (m)','FontSize',18) 15 | ylabel('y axis (m)','FontSize',18) 16 | zlabel('z axis (m)','FontSize',18) 17 | set(gca,'FontSize',18); 18 | title(aa,'MLat w/o filtering') 19 | hold on 20 | 21 | ab=subplot(2,1,2); 22 | plot3(ab,anchorLoc(:,1),anchorLoc(:,2),anchorLoc(:,3),'ko','MarkerSize',8,'lineWidth',2,'MarkerFaceColor','k'); 23 | grid on 24 | hold on 25 | curve1 = animatedline('Color','r','Marker','o'); 26 | axis([-1 4 -1 4.9 0 3]); 27 | ax1 = gca; 28 | ax1.XTick = (0:0.3:3); 29 | ax1.YTick = (0:0.3:3.9); 30 | ax1.ZTick = (0:1:3); 31 | xlabel('x axis (m)','FontSize',18) 32 | ylabel('y axis (m)','FontSize',18) 33 | zlabel('z axis (m)','FontSize',18) 34 | set(gca,'FontSize',18); 35 | title(ab,'MLat with filtering') 36 | hold on -------------------------------------------------------------------------------- /UltraSonic_study/definedFigure2.m: -------------------------------------------------------------------------------- 1 | function [aa,ab] = definedFigure2(anchorLoc,figureNumber,MultilaterationAlgorithmName) 2 | 3 | figure('Name',MultilaterationAlgorithmName); 4 | aa=subplot(2,1,1); 5 | plot3(aa,anchorLoc(:,1),anchorLoc(:,2),anchorLoc(:,3),'ko','MarkerSize',8,'lineWidth',2,'MarkerFaceColor','k'); 6 | grid on 7 | hold on 8 | curve = animatedline('Color','r','Marker','o'); 9 | axis([-1 4 -1 4.9 0 3]); 10 | ax = gca; 11 | ax.XTick = (0:0.3:3); 12 | ax.YTick = (0:0.3:3.9); 13 | ax.ZTick = (0:1:3); 14 | xlabel('x axis (m)','FontSize',18) 15 | ylabel('y axis (m)','FontSize',18) 16 | zlabel('z axis (m)','FontSize',18) 17 | set(gca,'fontsize',18); 18 | title(aa,'MLat w/o filtering') 19 | hold on 20 | 21 | ab=subplot(2,1,2); 22 | plot3(ab,anchorLoc(:,1),anchorLoc(:,2),anchorLoc(:,3),'ko','MarkerSize',8,'lineWidth',2,'MarkerFaceColor','k'); 23 | grid on 24 | hold on 25 | curve1 = animatedline('Color','r','Marker','o'); 26 | axis([-1 4 -1 4.9 0 3]); 27 | ax1 = gca; 28 | ax1.XTick = (0:0.3:3); 29 | ax1.YTick = (0:0.3:3.9); 30 | ax1.ZTick = (0:1:3); 31 | xlabel('x axis (m)','FontSize',18) 32 | ylabel('y axis (m)','FontSize',18) 33 | zlabel('z axis (m)','FontSize',18) 34 | set(gca,'fontsize',18); 35 | title(ab,'MLat with filtering') 36 | hold on -------------------------------------------------------------------------------- /plot_data2.m: -------------------------------------------------------------------------------- 1 | function plot_data2(figureNumber, PMlat, PKF, PR, i, k, PT, aa, ab) 2 | 3 | %% definingtions 4 | % FigureNumber = FN 5 | % PointsMultilateration = PMlat 6 | % PointsKF = PKF 7 | % Real Position = PR 8 | % Iteration I = i 9 | % Iteration k = k 10 | % Plot Type = PT 11 | % Subfigure 1 multilateration result = aa 12 | % Subfigure 2 multilateration filter result = ab 13 | %% Ploting funtion parts 14 | 15 | LineDrawing1 = [PMlat(i,1) PMlat(i,2) PMlat(i,3);... 16 | PMlat(i+1,1) PMlat(i+1,2) PMlat(i+1,3)]; 17 | LineDrawing2 = [PKF(i,1) PKF(i,2) PKF(i,3);... 18 | PKF(i+1,1) PKF(i+1,2) PKF(i+1,3)]; 19 | LineDrawing5 = [PR(k,:); PR(k-1,:)]; 20 | 21 | if PT == 1 22 | az = 0; el = 90; 23 | elseif PT == 2 24 | az = 90; el = 0; 25 | elseif PT == 0 26 | az = -50; el = 30; 27 | elseif PT == 3 28 | az = 180; el = 0; 29 | end 30 | 31 | view(aa,az,el); 32 | view(ab,az,el); 33 | 34 | line(aa,LineDrawing1(:,1),LineDrawing1(:,2),LineDrawing1(:,3),'Color','r');... 35 | line(aa,LineDrawing5(:,1),LineDrawing5(:,2),LineDrawing5(:,3),'Color','m'); 36 | line(ab,LineDrawing2(:,1),LineDrawing2(:,2),LineDrawing2(:,3),'Color','b');... 37 | line(ab,LineDrawing5(:,1),LineDrawing5(:,2),LineDrawing5(:,3),'Color','m'); -------------------------------------------------------------------------------- /UltraSonic_study/plot_data2.m: -------------------------------------------------------------------------------- 1 | function plot_data2(figureNumber, PMlat, PKF, PR, i, k, PT, aa, ab) 2 | 3 | %% definingtions 4 | % FigureNumber = FN 5 | % PointsMultilateration = PMlat 6 | % PointsKF = PKF 7 | % Real Position = PR 8 | % Iteration I = i 9 | % Iteration k = k 10 | % Plot Type = PT 11 | % Subfigure 1 multilateration result = aa 12 | % Subfigure 2 multilateration filter result = ab 13 | %% Ploting funtion parts 14 | 15 | LineDrawing1 = [PMlat(i,1) PMlat(i,2) PMlat(i,3);... 16 | PMlat(i+1,1) PMlat(i+1,2) PMlat(i+1,3)]; 17 | LineDrawing2 = [PKF(i,1) PKF(i,2) PKF(i,3);... 18 | PKF(i+1,1) PKF(i+1,2) PKF(i+1,3)]; 19 | LineDrawing5 = [PR(k+1,:); PR(k,:)]; 20 | 21 | if PT == 1 22 | az = 0; el = 90; 23 | elseif PT == 2 24 | az = 90; el = 0; 25 | elseif PT == 0 26 | az = -37.5; el = 30; 27 | elseif PT == 3 28 | az = 180; el = 0; 29 | end 30 | 31 | view(aa,az,el); 32 | view(ab,az,el); 33 | 34 | line(aa,LineDrawing1(:,1),LineDrawing1(:,2),LineDrawing1(:,3),'Color','r');... 35 | line(aa,LineDrawing5(:,1),LineDrawing5(:,2),LineDrawing5(:,3),'Color','m'); 36 | line(ab,LineDrawing2(:,1),LineDrawing2(:,2),LineDrawing2(:,3),'Color','b');... 37 | line(ab,LineDrawing5(:,1),LineDrawing5(:,2),LineDrawing5(:,3),'Color','m'); -------------------------------------------------------------------------------- /UltraSonic_study/preDefinedMatrix2.m: -------------------------------------------------------------------------------- 1 | function [A,Q,R,H] = preDefinedMatrix2(dt,algorithm) 2 | 3 | 4 | % State transition matrix 5 | % State prediction 6 | A = [ 1 0 0 dt 0 0 dt^2/2 0 0 ; 7 | 0 1 0 0 dt 0 0 dt^2/2 0 ; 8 | 0 0 1 0 0 dt 0 0 dt^2/2 ; 9 | 0 0 0 1 0 0 dt 0 0 ; 10 | 0 0 0 0 1 0 0 dt 0 ; 11 | 0 0 0 0 0 1 0 0 dt ; 12 | 0 0 0 0 0 0 1 0 0 ; 13 | 0 0 0 0 0 0 0 1 0 ; 14 | 0 0 0 0 0 0 0 0 1]; 15 | 16 | Q = [.02^2 0 0 0 0 0 0 0 0; 17 | 0 .02^2 0 0 0 0 0 0 0; 18 | 0 0 .12^2 0 0 0 0 0 0; 19 | 0 0 0 .01^2 0 0 0 0 0; 20 | 0 0 0 0 .01^2 0 0 0 0; 21 | 0 0 0 0 0 .1^2 0 0 0; 22 | 0 0 0 0 0 0 .01^2 0 0; 23 | 0 0 0 0 0 0 0 .01^2 0; 24 | 0 0 0 0 0 0 0 0 .1^2]; 25 | 26 | if strcmp(algorithm,'Least_square')||strcmp(algorithm,'Trilateration') 27 | r1 = 0.1063; 28 | r2 = 0.1012; 29 | r3 = 0.3558; 30 | elseif strcmp(algorithm,'Recursive_Trilateration') 31 | r1 = 0.2032; 32 | r2 = 0.0944; 33 | r3 = 0.6848; 34 | elseif strcmp(algorithm,'Gradient_Descent') 35 | r1 = 0.1503; 36 | r2 = 0.1298; 37 | r3 = 0.2314; 38 | end 39 | r7 = 0; 40 | r8 = 0; 41 | r9 = 1.0394; 42 | R = diag([r1 r2 r3 0 0 0 r7 r8 r9]); 43 | H = diag([1 1 1 1 1 1 1 1 1]); -------------------------------------------------------------------------------- /Rotation/main_angles.m: -------------------------------------------------------------------------------- 1 | clear all 2 | close all 3 | clc 4 | 5 | %fileName = 'C:\Users\moham\Desktop\tests\20-06\data.xlsx'; 6 | %[acc,pos,dis,gyro,posReal] = readData(fileName); 7 | acc = []; 8 | gyro = []; 9 | 10 | %load('acc.mat','acc'); 11 | %load('gyro.mat','gyro'); 12 | %acc = xlsread('test.xlsx','acc','A:C'); 13 | %gyro = xlsread('test.xlsx','gyro','A:C'); 14 | load('testing\sensor_rotation_data.mat'); 15 | % load('C:\Users\moham\Desktop\tests\03-07\test\example.mat'); 16 | drift_gyro = [-0.000304 0.002140 -0.000540]; 17 | drift_acc = [0.154200 -0.138603 0.001707]; 18 | 19 | % acc = acc/norm(acc,inf); 20 | % gyro = gyro/norm(gyro,inf); 21 | 22 | gyro = gyro - repmat(drift_gyro,length(gyro),1); 23 | acc = acc - repmat(drift_acc,length(acc),1); 24 | 25 | 26 | axis = [1 1 1]; 27 | 28 | angle = []; 29 | dt = 0.1; 30 | filtered_angles = []; 31 | earth = [0 0 9.8]'; 32 | for i=1:length(acc) 33 | if i==1 34 | pre_gyro_angle = [0 0 0]; 35 | angle = [0 0 0]; 36 | else 37 | [filtered_angles, gyro_angle, acc_angles] = angles(acc(i,:),gyro(i,:),dt,pre_gyro_angle); 38 | angle(i,:) = filtered_angles; 39 | pre_gyro_angle = gyro_angle; 40 | all_angles(i,:) = [gyro_angle acc_angles]; 41 | end 42 | 43 | [Rx Ry Rz R] = rotationMat(angle); 44 | 45 | R1 = Rz*Ry*Rx; 46 | % R = [cos(angle(i,2))*cos(angle(i,3)) sin(angle(i,1))*sin(angle(i,2))*cos(angle(i,3))-cos(angle(i,1))*sin(angle(i,3)) cos(angle(i,1))*sin(angle(i,2))*cos(angle(i,3))+sin(angle(i,1))*sin(angle(i,3)); 47 | % cos(angle(i,2))*sin(angle(i,3)) sin(angle(i,1))*sin(angle(i,2))*sin(angle(i,3))+cos(angle(i,1))*cos(angle(i,3)) cos(angle(i,1))*sin(angle(i,2))*sin(angle(i,3))-sin(angle(i,1))*cos(angle(i,3)); 48 | % -sin(angle(i,2)) sin(angle(i,1))*cos(angle(i,2)) cos(angle(i,1))*cos(angle(i,2)) ]; 49 | 50 | 51 | acc2(i,:) = (R*acc(i,:)')' - (R\earth)'; 52 | acc2(i,:) = acc2(i,:)/norm(acc(i,:)); 53 | acc3(i) = norm(acc(i,:)); 54 | acc4(i) = norm(acc2(i,:)); 55 | end 56 | 57 | plot_angles(all_angles,angle,dt); 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /preDefinedMatrix.m: -------------------------------------------------------------------------------- 1 | function [A,C,Q,R,H] = preDefinedMatrix(dt,algorithm,path) 2 | 3 | 4 | % State transition matrix 5 | % State prediction 6 | A = [ 1 0 0 dt 0 0 dt^2/2 0 0 ; 7 | 0 1 0 0 dt 0 0 dt^2/2 0 ; 8 | 0 0 1 0 0 dt 0 0 dt^2/2 ; 9 | 0 0 0 1 0 0 dt 0 0 ; 10 | 0 0 0 0 1 0 0 dt 0 ; 11 | 0 0 0 0 0 1 0 0 dt ; 12 | 0 0 0 0 0 0 1 0 0 ; 13 | 0 0 0 0 0 0 0 1 0 ; 14 | 0 0 0 0 0 0 0 0 1]; 15 | 16 | 17 | 18 | % Measurement matrix 19 | % The expected measurement given the predicted state 20 | C = [1 0 0 0 0 0; 21 | 0 1 0 0 0 0; 22 | 0 0 1 0 0 0; 23 | 0 0 0 0 0 0 24 | 0 0 0 0 0 0 25 | 0 0 0 0 0 0 26 | 0 0 0 1 0 0; 27 | 0 0 0 0 1 0; 28 | 0 0 0 0 0 1]; 29 | 30 | 31 | 32 | if strcmp(algorithm,'Gradient_Descent') 33 | if strcmp(path,'pathOption1.mat') 34 | r1 = 0.0848; 35 | r2 = 0.1555; 36 | r3 = 0.1139; 37 | r7 = 7.001; 38 | r8 = 11.7603; 39 | r9 = .0639; 40 | elseif strcmp(path,'pathOption2.mat') 41 | r1 = 0.1692; 42 | r2 = 0.1991; 43 | r3 = 0.1507; 44 | r7 = 2.6988; 45 | r8 = 2.4881; 46 | r9 = .0657; 47 | elseif strcmp(path,'pathOption3.mat') 48 | r1 = 0.2572; 49 | r2 = 0.1602; 50 | r3 = 0.1612; 51 | r7 = 2.925; 52 | r8 = 1.5712; 53 | r9 = .0567; 54 | else 55 | r1 = 0.3645; 56 | r2 = 0.3755; 57 | r3 = 0.3027; 58 | r7 = 2.5505; 59 | r8 = 1.7688; 60 | r9 = 1.4553; 61 | end 62 | elseif strcmp(algorithm,'Recursive_Least_square') 63 | if strcmp(path,'pathOption1.mat') 64 | r1 = 0.1046; 65 | r2 = 0.1947; 66 | r3 = 0.3307; 67 | r7 = 7.0004; 68 | r8 = 11.7610; 69 | r9 = .063; 70 | elseif strcmp(path,'pathOption2.mat') 71 | r1 = 0.3129; 72 | r2 = 0.1721; 73 | r3 = 1.8631; 74 | r7 = 2.7077; 75 | r8 = 2.4932; 76 | r9 = .0657; 77 | elseif strcmp(path,'pathOption3.mat') 78 | r1 = 0.3176; 79 | r2 = 0.1269; 80 | r3 = 1.9883; 81 | r7 = 2.9103; 82 | r8 = 1.5768; 83 | r9 = .0515; 84 | else 85 | r1 = 0.3040; 86 | r2 = 0.3086; 87 | r3 = 1.4204; 88 | r7 = 2.5563; 89 | r8 = 1.7695; 90 | r9 = 1.4553; 91 | end 92 | elseif strcmp(algorithm,'Least_square') 93 | if strcmp(path,'pathOption1.mat') 94 | r1 = 0.1183; 95 | r2 = 0.1772; 96 | r3 = 0.3891; 97 | r7 = 7.0004; 98 | r8 = 11.7613; 99 | r9 = .0628; 100 | elseif strcmp(path,'pathOption2.mat') 101 | r1 = 0.2258; 102 | r2 = 0.2414; 103 | r3 = 0.4; 104 | r7 = 2.6970; 105 | r8 = 2.4861; 106 | r9 = .064; 107 | elseif strcmp(path,'pathOption3.mat') 108 | r1 = 0.2727; 109 | r2 = 0.1729; 110 | r3 = 0.4; 111 | r7 = 2.987; 112 | r8 = 1.5754; 113 | r9 = .0253; 114 | else 115 | r1 = 0.3589; 116 | r2 = 0.3198; 117 | r3 = 0.7341; 118 | r7 = 2.5520; 119 | r8 = 1.7692; 120 | r9 = 1.4552; 121 | end 122 | end 123 | 124 | R = diag([r1 r2 r3 0 0 0 r7 r8 r9]); 125 | Q = [0.1^2 0 0 0 0 0 0 0 0; 126 | 0 0.1^2 0 0 0 0 0 0 0; 127 | 0 0 0.01^2 0 0 0 0 0 0; 128 | 0 0 0 0.5^2 0 0 0 0 0; 129 | 0 0 0 0 0.4^2 0 0 0 0; 130 | 0 0 0 0 0 0.1^2 0 0 0; 131 | 0 0 0 0 0 0 0.5^2 0 0; 132 | 0 0 0 0 0 0 0 0.4^2 0; 133 | 0 0 0 0 0 0 0 0 0.1^2]; 134 | 135 | H = diag([1 1 1 1 1 1 1 1 1]); -------------------------------------------------------------------------------- /UltraSonic_study/main2.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all 3 | clc 4 | 5 | %% choosen path 6 | % options are the following: 7 | % this movement focus on z axis 8 | 9 | load('input.mat'); 10 | 11 | %% calculating positions using mlat (3 algorithms) 12 | % Algorithms are four: 1- Gradient Descent 13 | % 2- Recursive Least Square 14 | % 3- Least Square 15 | % we need to choose algorithm: 16 | 17 | % Algorithm = 'Gradient_Descent'; 18 | Algorithm = 'Recursive_Trilateration'; 19 | % Algorithm = 'Least_square'; 20 | 21 | %% Options by running the code 22 | % yes = 1 23 | % No = 0 24 | 25 | without_IMU = 0; 26 | Object_surround_by_point = 0; 27 | Collision_detection = 0; 28 | printout_video_of_simulation = 0; 29 | 30 | %% Beginning of the Code 31 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Code Start %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 34 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 35 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 36 | %% Variable definitions 37 | % positionEstimate: estimated postion usig an algorithm of multilateration 38 | % filtered_angles: the angles that will be using in rotation matrix 39 | % dt: Time interval the system in case 4 anchros and for only 40 | % sensors in case 5 anchors 41 | % bounds: when using gradient decent to define bounds that 42 | % estimated postion should be in 43 | % iteration: loop iteration of Kalman filter 44 | % anchorsNumber: number of anchors used in the system 45 | 46 | positionEstimate=[]; 47 | iteration = length(acc); 48 | anchorsNumber = size(dis,2); 49 | dt = 0.25; 50 | %% starting with process 51 | [A,Q,R,H] = preDefinedMatrix2(dt,Algorithm); 52 | tic 53 | for i=1:iteration 54 | 55 | positionEstimate(i,:) = mlat.do_main(Algorithm, anchorLoc, dis(i,:)); 56 | %% Initial values for i==1 for kalman filtering and angles calculation 57 | if (i==1) 58 | x_t = [positionEstimate(i,:) 0 0 0 0 0 0]'; 59 | 60 | pHat = [dt^4/4 0 0 dt^3/2 0 0 dt^2/2 0 0; 61 | 0 dt^4/4 0 0 dt^3/2 0 0 dt^2/2 0; 62 | 0 0 dt^4/4 0 0 dt^3/2 0 0 dt^2/2; 63 | dt^3/2 0 0 dt^2 0 0 dt 0 0; 64 | 0 dt^3/2 0 0 dt^2 0 0 dt 0; 65 | 0 0 dt^3/2 0 0 dt^2 0 0 dt; 66 | dt^2/2 0 0 dt 0 0 1 0 0; 67 | 0 dt^2/2 0 0 dt 0 0 1 0; 68 | 0 0 dt^2/2 0 0 dt 0 0 1]; 69 | else 70 | x_t = X; 71 | pHat = P; 72 | end 73 | 74 | %% reading values position after applying Multilateration algorithms and acceleration 75 | if without_IMU == 1 76 | Y_t = [positionEstimate(i,:) USz(i) 0 0 0]'; 77 | else 78 | Y_t = [positionEstimate(i,:) USz(i) acc(i,1:3)]'; 79 | end 80 | 81 | %% Kalman Filtering 82 | alpha = 0.11; 83 | if USz(i) > 1.0 84 | alpha = 1-alpha; 85 | end 86 | [C] = errorMeasuresMatrix(alpha); 87 | [X, P] = ekf( A, C, Q, H, R , x_t, Y_t, pHat); 88 | 89 | %% Results saving 90 | F(i,1:3) = positionEstimate(i,:); 91 | F(i,4:6) = X(1:3); 92 | aAndv(i,:) = X(4:9); 93 | GG(i,:) = abs(F(i,1:3)-posReal(i,1:3)); 94 | [eM eMKF] = errorFun(posReal,F,i,i); 95 | ErrorMatrix(i,1:2) = [eM eMKF]; 96 | end 97 | toc 98 | %% Error Calculation for x,y,z mean, standard deviation 99 | ErrorM = ErrorCalculation(F(:,4:6),posReal, anchorsNumber); 100 | MeanErrorMX = mean(ErrorM(:,1)); 101 | MeanErrorMY = mean(ErrorM(:,2)); 102 | MeanErrorMZ = mean(ErrorM(:,3)); 103 | SDErrorMX = std(ErrorM(:,1)); 104 | SDErrorMY = std(ErrorM(:,2)); 105 | SDErrorMZ = std(ErrorM(:,3)); 106 | 107 | %% Analysis for Error 108 | meanAlgorithm = mean(ErrorMatrix(9:end,1)); 109 | maxAlgorithm = max(ErrorMatrix(9:end,1)); 110 | minAlgorithm = min(ErrorMatrix(9:end,1)); 111 | fprintf('Error Analysis without Kalman Filter\n mean: %f \t max: %f \t min: %f \n\n'... 112 | ,meanAlgorithm, maxAlgorithm, minAlgorithm); 113 | fprintf('++++++++++++++++++++++++++++++++++++++++++++++++++\n\n'); 114 | meanKF_Algorithm = mean(ErrorMatrix(9:end,2)); 115 | maxKF_Algorithm = max(ErrorMatrix(9:end,2)); 116 | minKF_Algorithm = min(ErrorMatrix(9:end,2)); 117 | fprintf('Error Analysis with Kalman Filter\n mean: %f \t max: %f \t min: %f \n'... 118 | ,meanKF_Algorithm, maxKF_Algorithm, minKF_Algorithm'); 119 | 120 | %% Plotting section 121 | %plotting type 122 | plotType = 0; % 0 => 3D 123 | % 1 => XY plane 124 | % 2 => YZ plane 125 | % 3 => XZ plane 126 | 127 | [aa,ab] = definedFigure2(anchorLoc,1,Algorithm); 128 | 129 | %vertices and faces of cube 130 | if Object_surround_by_point == 1 131 | ObjCentralPoint = [F(90,4:5) 1]; 132 | [V Fa] = cubeData(F(1,:),MeanErrorMX,MeanErrorMY,MeanErrorMZ,SDErrorMX,SDErrorMY,SDErrorMZ); 133 | else 134 | ObjCentralPoint = [-50 -50 -50]; 135 | [V Fa] = cubeData([0 0 0 ObjCentralPoint],MeanErrorMX,MeanErrorMY,MeanErrorMZ,SDErrorMX,SDErrorMY,SDErrorMZ); 136 | end 137 | 138 | [V2 Fa2] = cubeData([0 0 0 ObjCentralPoint],0.01,0.01,0.01,0.05,0.05,ObjCentralPoint(3)/3); 139 | 140 | S1.Vertices = V; 141 | S1.Faces = Fa; 142 | S1.FaceVertexCData = jet(size(V,1)); 143 | S1.FaceColor = 'interp'; 144 | S1.FaceAlpha = .1; 145 | g = hgtransform; 146 | S1Obj = patch(S1,'Parent',g); 147 | 148 | S2.Vertices = V2; 149 | S2.Faces = Fa2; 150 | S2.FaceVertexCData = jet(size(V2,1)); 151 | S2.FaceColor = 'red'; 152 | S2.FaceAlpha = .1; 153 | S2Obj = patch(S2); 154 | %pause(5) 155 | 156 | for i=1:length(F)-1 157 | plot_data2(1, F(:,1:3) , F(:,4:6) , posReal, i, i, plotType, aa, ab) 158 | if Object_surround_by_point == 1 159 | if i==1 160 | S1Obj = patch(S1,'Parent',g); 161 | gg = 0; 162 | else 163 | gg = gg + F(i,4:6)-F(i-1,4:6); 164 | g.Matrix = makehgtform('translate',gg); 165 | drawnow; 166 | set(S1Obj,'Vertices',get(S1Obj,'Vertices')); 167 | pause(0.1) 168 | end 169 | end 170 | 171 | % frame for video incase we need to save 172 | if printout_video_of_simulation == 1 173 | frameVideo(i) = getframe(figure(1)); 174 | end 175 | 176 | % Do collision detection 177 | if Collision_detection == 1 178 | collisionFlag = colldetect(F(i,4:6),ObjCentralPoint,[SDErrorMX SDErrorMY SDErrorMZ],[0.05,0.05,ObjCentralPoint(3)/3]); 179 | 180 | drawnow; 181 | 182 | if collisionFlag 183 | str = {'Collision!','Warning'}; 184 | t = text(2,2,3,str,'FontSize',30); 185 | break; 186 | end 187 | end 188 | end 189 | %% create video file 190 | if printout_video_of_simulation == 1 191 | movie(figure(2),frameVideo,1); 192 | Vid = VideoWriter('simulation.avi'); 193 | Vid.FrameRate = 10; 194 | open(Vid); 195 | writeVideo(Vid,frameVideo); 196 | close(Vid); 197 | end -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | close all 2 | clear all 3 | clc 4 | 5 | addpath('Rotation'); 6 | addpath('ErrorCalculation'); 7 | %% choosen path 8 | %options are the following: 9 | 10 | % 1: reads of 5 anchors with different heights using update firmware with 4Hz update rate of UWB 11 | % raw data system and have the following path (Experiment 3) 12 | % --------------------- 13 | % | | 14 | % | | 15 | % --------------------- 16 | 17 | % 2: reads of 4 anchors with the same heights using original firmware with 10Hz update rate of 18 | % UWB raw data system and have the following path (Experiment1) 19 | % --------------------- 20 | % | | 21 | % | | 22 | % --------------------- 23 | 24 | % 3: reads of 4 anchors with the same heights using original firmware with 10Hz update rate of 25 | % UWB raw data system and have the following path (Extra1) 26 | % | 27 | % --------------------- 28 | % | 29 | % --------------------- 30 | % | 31 | % --------------------- 32 | 33 | % 4: reads of 4 anchors with different heights using original firmware with 10Hz update rate of 34 | % UWB raw data system and have the following path (Experiment 2) 35 | % --------------------- 36 | % | | 37 | % | | 38 | % 39 | % 5: reads of 4 anchors with different heights using original firmware with 10Hz update rate of 40 | % UWB raw data system and have the following path (Extra 2) 41 | % ---------- 42 | % | \ / | 43 | % | \ / | 44 | % | \/ | 45 | % | /\ | 46 | % | / \ | 47 | % | / \ | 48 | % ---------- 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | 51 | path = 'pathOption1.mat'; %Exp3 52 | % path = 'pathOption2.mat'; %Exp1 53 | % path = 'pathOption3.mat'; %Extra1 54 | % path = 'pathOption4.mat'; %Exp2 55 | % path = 'pathOption5.mat'; %Extra2 56 | 57 | load(path); 58 | 59 | %% calculating positions using mlat (3 algorithms) 60 | % Algorithms are four: 1- Gradient Descent 61 | % 2- Recursive Least Square 62 | % 3- Least Square 63 | % we need to choose algorithm: 64 | 65 | Algorithm = 'Gradient_Descent'; 66 | % Algorithm = 'Recursive_Least_square'; 67 | % Algorithm = 'Least_square'; 68 | 69 | %% Options by running the code 70 | % yes = 1 71 | % No = 0 72 | without_IMU = 0; 73 | Object_surround_by_point = 0; 74 | Collision_detection = 0; 75 | printout_video_of_simulation = 0; 76 | 77 | %% Beginning of the Code 78 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 79 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 80 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Code Start %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 81 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 83 | %% Variable definitions 84 | % positionEstimate: estimated postion usig an algorithm of multilateration 85 | % filtered_angles: the angles that will be using in rotation matrix 86 | % dt: Time interval the system in case 4 anchros and for only 87 | % sensors in case 5 anchors 88 | % bounds: when using gradient decent to define bounds that 89 | % estimated postion should be in 90 | % iteration: loop iteration of Kalman filter 91 | % anchorsNumber: number of anchors used in the system 92 | 93 | positionEstimate=[]; 94 | filtered_angles = []; 95 | earth = [0 0 9.8]'; 96 | iteration = length(acc); 97 | anchorsNumber = size(dis,2); 98 | 99 | %% Synchornization procedure for reads from UWB system and sensors 100 | %defining timeStamp for distances in matrix dtDis 101 | if anchorsNumber == 5 102 | for i=2:length(dis) 103 | dtT(i) = tI(i,2) - tI(i-1,2); 104 | if dtT(i)<0 105 | dtT(i) = dtT(i)+60; 106 | end 107 | if dtT(i)>1 108 | break; 109 | end 110 | end 111 | dt=1/SampleRateSensors; 112 | else 113 | dt=1/SampleRateSensors; 114 | end 115 | 116 | %% starting with process 117 | 118 | [A,C,Q,R,H] = preDefinedMatrix(dt,Algorithm,path); 119 | k=1; 120 | kIteration = length(dis); 121 | 122 | for i=1:iteration 123 | if i==1 124 | prePos = []; 125 | else 126 | % prePos = []; 127 | prePos = F(i-1,1:3); 128 | end 129 | if anchorsNumber == 5 130 | if k<=kIteration 131 | positionEstimate(k,:) = mlat.do_main(Algorithm, anchorLoc, dis(k,:), prePos); 132 | end 133 | else 134 | positionEstimate(i,:) = mlat.do_main(Algorithm, anchorLoc, dis(i,:), prePos); 135 | end 136 | %% Initial values for i==1 for kalman filtering and angles calculation 137 | if (i==1) 138 | if anchorsNumber==5 139 | x_t = [positionEstimate(k,:) 0 0 0 0 0 0]'; 140 | else 141 | x_t = [positionEstimate(i,:) 0 0 0 0 0 0]'; 142 | end 143 | pHat = [dt^4/4 0 0 dt^3/2 0 0 dt^2/2 0 0; 144 | 0 dt^4/4 0 0 dt^3/2 0 0 dt^2/2 0; 145 | 0 0 dt^4/4 0 0 dt^3/2 0 0 dt^2/2; 146 | dt^3/2 0 0 dt^2 0 0 dt 0 0; 147 | 0 dt^3/2 0 0 dt^2 0 0 dt 0; 148 | 0 0 dt^3/2 0 0 dt^2 0 0 dt; 149 | dt^2/2 0 0 dt 0 0 1 0 0; 150 | 0 dt^2/2 0 0 dt 0 0 1 0; 151 | 0 0 dt^2/2 0 0 dt 0 0 1]; 152 | 153 | pre_gyro_angle = [0 0 0]; 154 | filtered_angles = [0 0 0]; 155 | Super_filtered_angles = [0 0 0]; 156 | else 157 | x_t = X; 158 | pHat = P; 159 | [filtered_angles, gyro_angle, acc_angles] = angles(acc(i,1:3),gyro(i,1:3),dt,pre_gyro_angle); 160 | pre_gyro_angle = gyro_angle; 161 | % if anchorsNumber==5 162 | % if k<=kIteration 163 | % Super_filtered_angles = (filtered_angles + [atan2(positionEstimate(k,2),positionEstimate(k,3)) atan2(positionEstimate(k,3),positionEstimate(k,1)) atan2(positionEstimate(k,2),positionEstimate(k,1))])/2; 164 | % end 165 | % else 166 | % Super_filtered_angles = (filtered_angles + [atan2(positionEstimate(i,2),positionEstimate(i,3)) atan2(positionEstimate(i,3),positionEstimate(i,1)) atan2(positionEstimate(i,2),positionEstimate(i,1))])/2; 167 | % end 168 | end 169 | %% calculate rotation matrix to delete gravity effect of accelerometer values 170 | [Rx Ry Rz Rot] = rotationMat(filtered_angles); 171 | %% acceleration without gravity value 172 | acc2(i,1:3) = (inv(Rot)*acc(i,1:3)')' - earth'; 173 | anorm = norm(acc2(i,1:3)); 174 | acc(i,1:3) = -acc2(i,1:3)/anorm; 175 | if without_IMU == 1 176 | acc(i,1:3) = [0 0 0]; 177 | end 178 | %% reading values position after applying Multilateration algorithms and acceleration 179 | if anchorsNumber==5 180 | j=0; 181 | if k<=kIteration 182 | if tI(k,1) == acc(i,5) 183 | if tI(k,2) == acc(i,6) 184 | Y_t = [positionEstimate(k,:) acc(i,1:3)]'; 185 | k=k+1; 186 | j=1; 187 | else 188 | Y_t = [positionEstimate(k,:)+(acc(i,1:3))*dt^2, acc(i,1:3)]'; 189 | end 190 | end 191 | end 192 | else 193 | Y_t = [positionEstimate(i,:) acc(i,1:3)]'; 194 | end 195 | 196 | %% Kalman Filtering and updating Uncertainty for measures and Estimation 197 | [X, P] = ekf( A, C, Q, H, R , x_t, Y_t, pHat); 198 | 199 | %% Results saving 200 | if anchorsNumber==5 201 | F(i,1:3) = positionEstimate(k-1,:); 202 | else 203 | F(i,1:3) = positionEstimate(i,:); 204 | end 205 | % diff(I*length(acc) + i,:) = F(i,1:3)-posReal(k-1,:); 206 | F(i,4:6) = X(1:3); 207 | 208 | %% Calculating error with Error Calculation 209 | if anchorsNumber ==5 210 | if j==1 211 | if k==2 212 | ErrorMatrix(k-1,:) = [0 0]; 213 | GG(i,:) = [0 0 0]; 214 | else 215 | [eM eMKF] = errorFun(posReal,F,i,k-1); 216 | ErrorMatrix(k-1,:) = [eM eMKF]; 217 | GG(i,:) = abs(F(i,1:3)-posReal(k-1,1:3)); 218 | end 219 | end 220 | else 221 | [eM eMKF] = errorFun(posReal,F,i,i); 222 | ErrorMatrix(i,1:2) = [eM eMKF]; 223 | end 224 | end 225 | %% Error Calculation for x,y,z mean, standard deviation 226 | ErrorM = ErrorCalculation(F(:,4:6),posReal, anchorsNumber); 227 | MeanErrorMX = mean(ErrorM(:,1)); 228 | MeanErrorMY = mean(ErrorM(:,2)); 229 | MeanErrorMZ = mean(ErrorM(:,3)); 230 | SDErrorMX = std(ErrorM(:,1)); 231 | SDErrorMY = std(ErrorM(:,2)); 232 | SDErrorMZ = std(ErrorM(:,3)); 233 | 234 | %% Analysis for Error 235 | meanAlgorithm = mean(ErrorMatrix(9:end,1)); 236 | maxAlgorithm = max(ErrorMatrix(9:end,1)); 237 | minAlgorithm = min(ErrorMatrix(9:end,1)); 238 | fprintf('Error Analysis without Kalman Filter\n mean: %f \t max: %f \t min: %f \n\n'... 239 | ,meanAlgorithm, maxAlgorithm, minAlgorithm); 240 | fprintf('++++++++++++++++++++++++++++++++++++++++++++++++++\n\n'); 241 | meanKF_Algorithm = mean(ErrorMatrix(9:end,2)); 242 | maxKF_Algorithm = max(ErrorMatrix(9:end,2)); 243 | minKF_Algorithm = min(ErrorMatrix(9:end,2)); 244 | fprintf('Error Analysis with Kalman Filter\n mean: %f \t max: %f \t min: %f \n'... 245 | ,meanKF_Algorithm, maxKF_Algorithm, minKF_Algorithm'); 246 | 247 | %% Plotting section 248 | %plotting type 249 | plotType = 0; % 0 => 3D 250 | % 1 => XY plane 251 | % 2 => YZ plane 252 | % 3 => XZ plane 253 | 254 | [aa,ab] = definedFigure2(anchorLoc,1,Algorithm); 255 | k=2; 256 | 257 | %vertices and faces of cube 258 | if Object_surround_by_point == 1 259 | ObjCentralPoint = [F(90,4:5) 1]; 260 | [V Fa] = cubeData(F(1,:),MeanErrorMX,MeanErrorMY,MeanErrorMZ,SDErrorMX,SDErrorMY,SDErrorMZ); 261 | else 262 | ObjCentralPoint = [-50 -50 -50]; 263 | [V Fa] = cubeData([0 0 0 ObjCentralPoint],MeanErrorMX,MeanErrorMY,MeanErrorMZ,SDErrorMX,SDErrorMY,SDErrorMZ); 264 | end 265 | [V2 Fa2] = cubeData([0 0 0 ObjCentralPoint],0.01,0.01,0.01,0.05,0.05,ObjCentralPoint(3)/3); 266 | 267 | S1.Vertices = V; 268 | S1.Faces = Fa; 269 | S1.FaceVertexCData = jet(size(V,1)); 270 | S1.FaceColor = 'interp'; 271 | S1.FaceAlpha = .1; 272 | g = hgtransform; 273 | S1Obj = patch(S1,'Parent',g); 274 | 275 | S2.Vertices = V2; 276 | S2.Faces = Fa2; 277 | S2.FaceVertexCData = jet(size(V2,1)); 278 | S2.FaceColor = 'red'; 279 | S2.FaceAlpha = .1; 280 | S2Obj = patch(S2); 281 | pause(5) 282 | 283 | for i=1:length(F)-1 284 | plot_data2(1, F(:,1:3) , F(:,4:6) , posReal, i, k, plotType, aa, ab) 285 | if Object_surround_by_point == 1 286 | if i==1 287 | S1Obj = patch(S1,'Parent',g); 288 | gg = 0; 289 | else 290 | gg = gg + F(i,4:6)-F(i-1,4:6); 291 | g.Matrix = makehgtform('translate',gg); 292 | drawnow; 293 | set(S1Obj,'Vertices',get(S1Obj,'Vertices')); 294 | pause(0.1) 295 | end 296 | end 297 | if anchorsNumber == 5 298 | if k==length(posReal)-1 299 | break; 300 | elseif tI(k,1) == acc(i,5) 301 | if tI(k,2) == acc(i,6) 302 | k=k+1; 303 | end 304 | end 305 | else 306 | k = i+1; 307 | end 308 | % frame for video incase we need to save 309 | if printout_video_of_simulation == 1 310 | frameVideo(i) = getframe(figure(1)); 311 | end 312 | 313 | % Do collision detection 314 | if Collision_detection == 1 315 | collisionFlag = colldetect(F(i,4:6),ObjCentralPoint,[MeanErrorMX MeanErrorMY MeanErrorMZ],[SDErrorMX SDErrorMY SDErrorMZ],[0.05,0.05,ObjCentralPoint(3)/3]); 316 | drawnow; 317 | if collisionFlag 318 | str = {'Collision!','Warning'}; 319 | t = text(2,2,3,str,'FontSize',30); 320 | break; 321 | end 322 | end 323 | end 324 | 325 | %% create video file 326 | if printout_video_of_simulation == 1 327 | movie(figure(2),frameVideo,1); 328 | Vid = VideoWriter('simulationCD.avi'); 329 | Vid.FrameRate = 30; 330 | Vid.Quality = 90; 331 | open(Vid); 332 | writeVideo(Vid,frameVideo); 333 | close(Vid); 334 | end -------------------------------------------------------------------------------- /UltraSonic_study/mlat.m: -------------------------------------------------------------------------------- 1 | classdef mlat 2 | % to run Mlat you need to use main funciton 3 | % > do_main('algorithm', anchor Location, distances, varargin) 4 | % > 'algorithms' are four: gradient descent, recursive trilateration, 5 | % simple least square, normal trilateration 6 | % > varargin: using bounds for gradient descent and weighting matrix (error 7 | % variance matrix) for recursive and simple trilateration algorithms 8 | % > MLAT is class contains 4 different algorithms for Multilateration and 9 | % those are: 10 | % 1- Gradient Decent using two funtions: 11 | % a- gd() 12 | % b- do_gdescent() 13 | % 2- Recursive Trilateration using three funtions: 14 | % a- RecTrilateration() 15 | % b- distancen() 16 | % c- lsrec() 17 | % d- do_RecTri() 18 | % 3- Simple Least Square Error algorithm using one funtion do_LLS() 19 | % 4- normal Trilateration algorithm using one funtion do_Tri() 20 | 21 | methods (Access = public ,Static) 22 | %% main function 23 | function position = do_main(Algorithm, anchor_Location, ranges_in, varargin) 24 | bounds_in = findBounds(anchor_Location); 25 | W = eye(length(ranges_in)); 26 | if length(varargin) ~= 0 27 | for i = 1:2:length(varargin{1}) 28 | switch (varargin{1}{i}) 29 | case 'bounds' 30 | bounds_in = varargin{1}{i+1}; 31 | case 'weight' 32 | W = varargin{1}{i+1}; 33 | case 'Null' 34 | continue; 35 | end 36 | end 37 | end 38 | 39 | switch Algorithm 40 | case 'Gradient_Descent' 41 | position = mlat.do_gdescent(anchor_Location, ranges_in, 'bounds',bounds_in); 42 | case 'Recursive_Trilateration' 43 | position = mlat.do_RecTri(anchor_Location', ranges_in, W); 44 | case 'Least_square' 45 | position = mlat.do_LLS(anchor_Location', ranges_in); 46 | case 'Trilateration' 47 | position = mlat.do_Tri(anchor_Location',ranges_in, W); 48 | end 49 | end 50 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 51 | %% Gradinet Descent Algorithm 52 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 53 | % Gradient Descent Algorithm % 54 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 55 | function result_table = gd(anchors_in, ranges_in, varargin) 56 | % varargin: 'bounds_in', 'n_trial', 'alpha', 'time_threshold' 57 | bounds_in = []; 58 | n_trial = 1000; 59 | alpha = 0.0001; 60 | time_threshold = 1/ n_trial; 61 | for i = 1:2:length(varargin{1}) 62 | switch (varargin{1}{i}) 63 | case 'bounds' 64 | bounds_in = varargin{1}{i+1}; 65 | case 'trial' 66 | n_trial = varargin{1}{i+1}; 67 | case 'alpha' 68 | alpha = varargin{1}{i+1}; 69 | case 'time' 70 | time_threshold = varargin{1}{i+1}; 71 | end 72 | end 73 | 74 | [n, dim] = size(anchors_in); 75 | bounds_temp = [anchors_in; bounds_in]; 76 | bounds(1, :) = min(bounds_temp); 77 | bounds(2, :) = max(bounds_temp); 78 | 79 | ranges = nan(1, n); 80 | result_table = nan(n_trial, dim + 1); 81 | 82 | for i = 1:n_trial 83 | estimator0 = nan(1, dim); 84 | % estimator0(1:2) = [1.5 2.2]; 85 | % estimator0(3) = (bounds(2, 3) - bounds(1, 3)) * rand + bounds(1, 3); 86 | for j = 1:dim 87 | estimator0(j) = (bounds(2, j) - bounds(1, j)) * rand + bounds(1, j); 88 | end 89 | estimator = estimator0; 90 | 91 | t0 = tic; 92 | while true 93 | for j = 1:n 94 | ranges(j) = norm(anchors_in(j, :) - estimator); 95 | end 96 | err = norm(ranges_in - ranges); 97 | 98 | delta = zeros(1, dim); 99 | for j = 1:n 100 | delta = delta + (ranges_in(j) - ranges(j)) / ranges(j) * (estimator - anchors_in(j, :)); 101 | end 102 | delta = 2 * alpha * delta; 103 | 104 | estimator_next = estimator - delta; 105 | for j = 1:n 106 | ranges(j) = norm(anchors_in(j, :) - estimator_next); 107 | end 108 | err_next = norm(ranges_in - ranges); 109 | if err_next < err 110 | estimator = estimator_next; 111 | else 112 | result_table(i, 1:dim) = estimator; 113 | result_table(i, end) = err; 114 | break; 115 | end 116 | if toc - t0 > time_threshold 117 | break; 118 | end 119 | end 120 | end 121 | end 122 | 123 | function estimator = do_gdescent(anchors_in, ranges_in, varargin) 124 | result_table = mlat.gd(anchors_in, ranges_in, varargin); 125 | [~, I] = min(result_table(:, end)); 126 | estimator = round(result_table(I, 1:end-1),2); 127 | end 128 | 129 | %% Simple Least Square Error algorithm 130 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 131 | % Least Square Error Simple Trilateration % 132 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 133 | function Nmat = do_LLS(anchors_in,ranges_in) 134 | A = []; b = []; 135 | [m n] = size(anchors_in); 136 | x = anchors_in(1,:); y = anchors_in(2,:); z = anchors_in(3,:); 137 | for i=2:n 138 | A(i-1,:) = [x(i)-x(1) , y(i)-y(1) , z(i)-z(1)]; 139 | 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; 140 | end 141 | %A = 2*A; 142 | Nmat = pinv(A'*A)*A'*b/2; 143 | end 144 | 145 | %% Recursive Trilateration 146 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 147 | % Recursive Trilateration % 148 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 149 | function Nmat = RecTrilateration(P,S,W) 150 | % paper "An algebraic solution to the multilateration problem" 151 | % Author: Norrdine, Abdelmoumen (norrdine@hotmail.de) 152 | % https://www.researchgate.net/publication/275027725_An_Algebraic_Solution_to_the_Multilateration_Problem 153 | % usage: [N1 N2] = RecTrilateration(P,S,W) 154 | % P = [P1 P2 P3 P4 ..] Reference points matrix 155 | % S = [s1 s2 s3 s4 ..] distance matrix. 156 | % W : Weights Matrix (Statistics). 157 | % N : calculated solution 158 | [mp,np] = size(P); 159 | ns = length(S); 160 | if (ns~=np) 161 | error('number of reference points and ranges and not the same'); 162 | end 163 | A=[]; b=[]; 164 | for i1=1:np 165 | x = P(1,i1); y = P(2,i1); z = P(3,i1); 166 | s = S(i1); 167 | A = [A ; 1 -2*x -2*y -2*z]; 168 | b= [b ; s^2-x^2-y^2-z^2 ]; 169 | end 170 | if (np==3) 171 | warning off; 172 | Xp= pinv(A)*b; % Gaussian elimination 173 | % or Xp=pinv(A)*b; P 174 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 175 | % depend only on the reference points 176 | % it could be computed only once 177 | xp = Xp(2:4,:); 178 | Z = null(A); 179 | z = Z(2:4,:); 180 | if rank (A)==3 181 | %Polynom coeff. 182 | a2 = z(1)^2 + z(2)^2 + z(3)^2 ; 183 | a1 = 2*(z(1)*xp(1) + z(2)*xp(2) + z(3)*xp(3))-Z(1); 184 | a0 = xp(1)^2 + xp(2)^2+ xp(3)^2-Xp(1); 185 | p = [a2 a1 a0]; 186 | t = roots(p); 187 | %solution 188 | N1 = Xp + t(1)*Z; 189 | N2 = Xp + t(2)*Z; 190 | Nmat(:,1) = N1; 191 | Nmat(:,2) = N2; 192 | end 193 | end 194 | A0 = A(1:3,:); 195 | if (np>3) 196 | P10=P(:,1:end-1);S10=S(:,1:end-1);W0=W(1:end-1,1:end-1); 197 | N0mat = mlat.RecTrilateration(P10,S10,W0); 198 | N01 = N0mat(:,1); 199 | N02 = N0mat(:,2); 200 | %select N0 201 | C = W'*W; 202 | Xpdw = pinv(A'*C*A)*A'*C*b; 203 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 204 | % depend only on the reference points 205 | % it could be computed only once 206 | NormErrorXpdw = Xpdw(1)-norm(Xpdw(2:4))^2; 207 | if (norm(Xpdw(2:4)-N01(2:4))= 1 and 258 | % size(An,2) = length(x). Wn is the weight associated with the new data, 259 | % which is typically equal to 1. If Wn is a scalar it applies to all new 260 | % equations; if it is a vector the i-th element of Wn applies to the i-th 261 | % equation. x and P are the output from the most recent recursive function 262 | % call. xn and Pn are the updated solution vector and P matrix for future 263 | % recursive calls. 264 | % 265 | % This function is useful when one wants to update a least squares solution 266 | % repeatedly as new data becomes available, such as after each pass through 267 | % some iterative process. 268 | % 269 | % Reference: "Modern Control Theory," 3rd ed., William L. Brogan 270 | % Prentice Hall, 1991. 271 | % 272 | % See also MLDIVIDE, LSCOV, LSQNONNEG. 273 | 274 | % D.C. Hanselman, University of Maine, Orono, ME 04469 275 | % MasteringMatlab@yahoo.com 276 | % Mastering MATLAB 7 277 | % 2006-11-8 278 | 279 | if nargin==1 % initialize recursive solution 280 | xn=varargin{1}(:); 281 | Pn=diag(1e12+zeros(size(xn))); 282 | 283 | elseif nargin==2 % initialize recursive solution 284 | xn=varargin{1}(:); 285 | if numel(varargin{2})~=1 286 | error('LSREC:scalar','Scalar Weight Required.') 287 | else 288 | W=varargin{2}; 289 | if W<=eps || W>1 290 | error('LSREC:OutofBound','Weight Must be Between 0 and 1.') 291 | end 292 | Pn=diag((1/W)+zeros(size(xn))); 293 | end 294 | 295 | elseif nargin==5 % recursive call 296 | 297 | yn=varargin{1}(:); % make sure yn is a column vector 298 | An=varargin{2}; 299 | Wn=varargin{3}(:); 300 | x=varargin{4}(:); 301 | P=varargin{5}; 302 | if length(yn)~=size(An,1) 303 | error('LSREC:nonconform',... 304 | 'yn Must Have as Many Rows as An.') 305 | end 306 | if size(An,2)~=length(x) 307 | error('LSREC:nonconform',... 308 | 'An Must Have as Many Columns as x has elements.') 309 | end 310 | if size(P,1)~=size(P,2) || size(P,1)~=length(x) 311 | error('LSREC:nonform',... 312 | 'P Must be a Square Matrix of Dimension Equal to length(x).') 313 | end 314 | if length(Wn)~=1 && length(Wn)~=length(yn) 315 | error('LSREC:conform',... 316 | 'Wn Must be a Scalar or Have the Same Number of Elements as yn.') 317 | end 318 | if any(Wn<=eps) || any(Wn>1) 319 | error('LSREC:OutofBound','Weights Must be Between 0 and 1.') 320 | end 321 | if numel(Wn)==1 % expand scalar weight if needed 322 | Wn=repmat(Wn,size(yn)); 323 | end 324 | 325 | K=P*An'/(An*P*An'+diag(1./Wn)); 326 | xn=x+K*(yn-An*x); 327 | if nargout>1 % compute new P 328 | Pn=P-K*An*P; 329 | end 330 | else 331 | error('LSREC:rhs','Recursive Calls Require 5 Input Arguments.') 332 | end 333 | end 334 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 335 | function NFinal = do_RecTri(P,S,W) 336 | Nmat = mlat.RecTrilateration(P,S,W); 337 | N1 = Nmat(:,1:end); 338 | [n m]=size(N1); 339 | for i1=1:m 340 | Nn = N1(:,i1); 341 | Nn = Nn(2:4); 342 | [Sn(i1,:) F(i1)] = mlat.distancen(Nn,P,S); 343 | diff(i1) = N1(1,i1) - norm(Nn).^2; 344 | end 345 | [Fmin Imin] = min(F); 346 | [NrecDel IDel] = min(N1(1,1:3)); 347 | NrecDel = N1(2:4,IDel); 348 | Nrec = N1(2:4,Imin); 349 | if imag(Nrec(2))==0 350 | NFinal = Nrec'; 351 | else 352 | for jj=1:size(N1,2) 353 | if imag(N1(2,jj))==0 354 | NFinal = N1(2:4,jj)'; 355 | end 356 | end 357 | end 358 | end 359 | 360 | %% Normal Trilateration 361 | %%%%%%%%%%%%%%%%% 362 | % Trilateration % 363 | %%%%%%%%%%%%%%%%% 364 | function Nmat = do_Tri(P,S,W) 365 | % paper "An algebraic solution to the multilateration problem" 366 | % Author: Norrdine, Abdelmoumen (norrdine@hotmail.de) 367 | % https://www.researchgate.net/publication/275027725_An_Algebraic_Solution_to_the_Multilateration_Problem 368 | % usage: [N1 N2] = Trilateration(P,S,W) 369 | % P = [P1 P2 P3 P4 ..] Reference points matrix 370 | % S = [s1 s2 s3 s4 ..] distance matrix. 371 | % W : Weights Matrix (Statistics). 372 | % N : calculated solution 373 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY!! 374 | [mp,np] = size(P); 375 | ns = length(S); 376 | if (ns~=np) 377 | error('Number of reference points and distances are different'); 378 | end 379 | A=[]; b=[]; 380 | for i1=1:np 381 | x = P(1,i1); y = P(2,i1); z = P(3,i1); 382 | s = S(i1); 383 | A = [A ; 1 -2*x -2*y -2*z]; 384 | b= [b ; s^2-x^2-y^2-z^2 ]; 385 | end 386 | 387 | if (np==3) 388 | warning off; 389 | Xp= A\b; % Gaussian elimination 390 | % or Xp=pinv(A)*b; 391 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 392 | % depend only on the reference points 393 | % it could be computed only once 394 | xp = Xp(2:4,:); 395 | Z = null(A,'r'); 396 | z = Z(2:4,:); 397 | if rank (A)==3 398 | %Polynom coeff. 399 | a2 = z(1)^2 + z(2)^2 + z(3)^2 ; 400 | a1 = 2*(z(1)*xp(1) + z(2)*xp(2) + z(3)*xp(3))-Z(1); 401 | a0 = xp(1)^2 + xp(2)^2+ xp(3)^2-Xp(1); 402 | p = [a2 a1 a0]; 403 | t = roots(p); 404 | 405 | %Solutions 406 | N1 = Xp + t(1)*Z; 407 | N2 = Xp + t(2)*Z; 408 | if N1(1)3) 416 | %Particular solution 417 | 418 | if W~=diag(ones(1,length(W))) 419 | C = W'*W; 420 | Xpdw =inv(A'*C*A)*A'*inv(C)*b; % Solution with Weights Matrix 421 | else 422 | Xpdw=inv(A'*A)*A'*b; % Solution without Weights Matrix 423 | end 424 | 425 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 426 | % depend only on the reference points 427 | % it could be computed only once 428 | N1 = Xpdw; 429 | N2 = N1; 430 | Nmat = N1(2:4)'; 431 | end 432 | end 433 | 434 | end 435 | 436 | end -------------------------------------------------------------------------------- /mlat.m: -------------------------------------------------------------------------------- 1 | classdef mlat 2 | % to run Mlat you need to use main funciton 3 | % > do_main('algorithm', anchor Location, distances, varargin) 4 | % > 'algorithms' are four: gradient descent, recursive trilateration, 5 | % simple least square, normal trilateration 6 | % > varargin: using bounds for gradient descent and weighting matrix (error 7 | % variance matrix) for recursive and simple trilateration algorithms 8 | % > MLAT is class contains 4 different algorithms for Multilateration and 9 | % those are: 10 | % 1- Gradient Decent using two funtions: 11 | % a- gd() 12 | % b- do_gdescent() 13 | % 2- Recursive Trilateration using three funtions: 14 | % a- RecTrilateration() 15 | % b- distancen() 16 | % c- lsrec() 17 | % d- do_RecTri() 18 | % 3- Simple Least Square Error algorithm using one funtion do_LLS() 19 | 20 | methods (Access = public ,Static) 21 | %% main function 22 | function position = do_main(Algorithm, anchor_Location, ranges_in, prePos, varargin) 23 | W = eye(length(ranges_in)); 24 | if length(varargin) ~= 0 25 | for i = 1:2:length(varargin{1}) 26 | switch (varargin{1}{i}) 27 | case 'bounds' 28 | bounds_in = varargin{1}{i+1}; 29 | case 'weight' 30 | W = varargin{1}{i+1}; 31 | case 'Null' 32 | continue; 33 | end 34 | end 35 | end 36 | 37 | switch Algorithm 38 | case 'Gradient_Descent' 39 | bounds_in = mlat.do_findBounds(anchor_Location, prePos); 40 | position = mlat.do_gdescent(anchor_Location, ranges_in, 'bounds', bounds_in); 41 | case 'Recursive_Least_square' 42 | position = mlat.do_RecTri(anchor_Location', ranges_in, W); 43 | case 'Least_square' 44 | position = mlat.do_LLS(anchor_Location', ranges_in); 45 | end 46 | end 47 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 48 | %% Gradinet Descent Algorithm 49 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 | % Gradient Descent Algorithm % 51 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 52 | function result_table = gd(anchors_in, ranges_in, varargin) 53 | % varargin: 'bounds_in', 'n_trial', 'alpha', 'time_threshold' 54 | bounds_in = []; 55 | n_trial = 1000; 56 | alpha = 0.0001; 57 | % time_threshold = 1/ n_trial; 58 | for i = 1:2:length(varargin{1}) 59 | switch (varargin{1}{i}) 60 | case 'bounds' 61 | bounds_in = varargin{1}{i+1}; 62 | case 'trial' 63 | n_trial = varargin{1}{i+1}; 64 | case 'alpha' 65 | alpha = varargin{1}{i+1}; 66 | % case 'time' 67 | % time_threshold = varargin{1}{i+1}; 68 | end 69 | end 70 | 71 | [n, dim] = size(anchors_in); 72 | bounds_temp = [anchors_in; bounds_in]; 73 | if isempty(bounds_in) 74 | bounds(1, :) = min(bounds_temp); 75 | bounds(2, :) = max(bounds_temp); 76 | else 77 | bounds = bounds_in; 78 | end 79 | ranges = nan(1, n); 80 | result_table = nan(n_trial, dim + 1); 81 | 82 | 83 | for i = 1:n_trial 84 | estimator0 = nan(1, dim); 85 | for j = 1:dim 86 | estimator0(j) = (bounds(2, j) - bounds(1, j)) * rand + bounds(1, j); 87 | end 88 | estimator = estimator0; 89 | 90 | t0 = tic; 91 | while true 92 | for j = 1:n 93 | ranges(j) = norm(anchors_in(j, :) - estimator); 94 | end 95 | err = norm(ranges_in - ranges); 96 | 97 | delta = zeros(1, dim); 98 | for j = 1:n 99 | delta = delta + (ranges_in(j) - ranges(j)) / ranges(j) * (estimator - anchors_in(j, :)); 100 | end 101 | delta = 2 * alpha * delta; 102 | 103 | estimator_next = estimator - delta; 104 | for j = 1:n 105 | ranges(j) = norm(anchors_in(j, :) - estimator_next); 106 | end 107 | err_next = norm(ranges_in - ranges); 108 | if err_next < err 109 | estimator = estimator_next; 110 | else 111 | result_table(i, 1:dim) = estimator; 112 | result_table(i, end) = err; 113 | break; 114 | end 115 | % if toc - t0 > time_threshold 116 | % break; 117 | % end 118 | end 119 | end 120 | end 121 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 122 | function bounds_in = do_findBounds(anchors_in, prePos) 123 | alpha = 0.35; 124 | if isempty(prePos) 125 | for i = 1:size(anchors_in, 2) 126 | bounds_in(1, i) = min(anchors_in(:, i)); % minimum boundary of ith axis 127 | bounds_in(2, i) = max(anchors_in(:, i)); % maximum boundary of ith axis 128 | end 129 | % hard coded minimum height (0 m) of search boundary 130 | bounds_in(1, end) = 0; 131 | else 132 | bounds_in(1,:) = prePos-[alpha alpha alpha]; 133 | bounds_in(2,:) = prePos+[alpha alpha alpha]; 134 | end 135 | end 136 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 137 | function estimator = do_gdescent(anchors_in, ranges_in, varargin) 138 | 139 | result_table = mlat.gd(anchors_in, ranges_in, varargin); 140 | [~, I] = min(result_table(:, end)); 141 | estimator = round(result_table(I, 1:end-1),2); 142 | end 143 | 144 | %% Simple Least Square Error algorithm 145 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 146 | % Least Square Error Simple Trilateration % 147 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 148 | function Nmat = do_LLS(anchors_in,ranges_in) 149 | A = []; b = []; 150 | [m n] = size(anchors_in); 151 | x = anchors_in(1,:); y = anchors_in(2,:); z = anchors_in(3,:); 152 | for i=2:n 153 | A(i-1,:) = [x(i)-x(1) , y(i)-y(1) , z(i)-z(1)]; 154 | 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; 155 | end 156 | %A = 2*A; 157 | Nmat = pinv(A'*A)*A'*b/2; 158 | end 159 | 160 | %% Recursive Least_square 161 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 162 | % Recursive Least_square % 163 | %%%%%%%%%%%%%%%%%%%%%%%%%%% 164 | function Nmat = RecTrilateration(P,S,W) 165 | % paper "An algebraic solution to the multilateration problem" 166 | % Author: Norrdine, Abdelmoumen (norrdine@hotmail.de) 167 | % https://www.researchgate.net/publication/275027725_An_Algebraic_Solution_to_the_Multilateration_Problem 168 | % usage: [N1 N2] = RecTrilateration(P,S,W) 169 | % P = [P1 P2 P3 P4 ..] Reference points matrix 170 | % S = [s1 s2 s3 s4 ..] distance matrix. 171 | % W : Weights Matrix (Statistics). 172 | % N : calculated solution 173 | [mp,np] = size(P); 174 | ns = length(S); 175 | if (ns~=np) 176 | error('number of reference points and ranges and not the same'); 177 | end 178 | A=[]; b=[]; 179 | for i1=1:np 180 | x = P(1,i1); y = P(2,i1); z = P(3,i1); 181 | s = S(i1); 182 | A = [A ; 1 -2*x -2*y -2*z]; 183 | b= [b ; s^2-x^2-y^2-z^2 ]; 184 | end 185 | if (np==3) 186 | warning off; 187 | Xp= pinv(A)*b; % Gaussian elimination 188 | % or Xp=pinv(A)*b; P 189 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 190 | % depend only on the reference points 191 | % it could be computed only once 192 | xp = Xp(2:4,:); 193 | Z = null(A); 194 | z = Z(2:4,:); 195 | if rank (A)==3 196 | %Polynom coeff. 197 | a2 = z(1)^2 + z(2)^2 + z(3)^2 ; 198 | a1 = 2*(z(1)*xp(1) + z(2)*xp(2) + z(3)*xp(3))-Z(1); 199 | a0 = xp(1)^2 + xp(2)^2+ xp(3)^2-Xp(1); 200 | p = [a2 a1 a0]; 201 | t = roots(p); 202 | %solution 203 | N1 = Xp + t(1)*Z; 204 | N2 = Xp + t(2)*Z; 205 | Nmat(:,1) = N1; 206 | Nmat(:,2) = N2; 207 | end 208 | end 209 | A0 = A(1:3,:); 210 | if (np>3) 211 | P10=P(:,1:end-1);S10=S(:,1:end-1);W0=W(1:end-1,1:end-1); 212 | N0mat = mlat.RecTrilateration(P10,S10,W0); 213 | N01 = N0mat(:,1); 214 | N02 = N0mat(:,2); 215 | %select N0 216 | C = W'*W; 217 | Xpdw = pinv(A'*C*A)*A'*C*b; 218 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 219 | % depend only on the reference points 220 | % it could be computed only once 221 | NormErrorXpdw = Xpdw(1)-norm(Xpdw(2:4))^2; 222 | if (norm(Xpdw(2:4)-N01(2:4))= 1 and 273 | % size(An,2) = length(x). Wn is the weight associated with the new data, 274 | % which is typically equal to 1. If Wn is a scalar it applies to all new 275 | % equations; if it is a vector the i-th element of Wn applies to the i-th 276 | % equation. x and P are the output from the most recent recursive function 277 | % call. xn and Pn are the updated solution vector and P matrix for future 278 | % recursive calls. 279 | % 280 | % This function is useful when one wants to update a least squares solution 281 | % repeatedly as new data becomes available, such as after each pass through 282 | % some iterative process. 283 | % 284 | % Reference: "Modern Control Theory," 3rd ed., William L. Brogan 285 | % Prentice Hall, 1991. 286 | % 287 | % See also MLDIVIDE, LSCOV, LSQNONNEG. 288 | 289 | % D.C. Hanselman, University of Maine, Orono, ME 04469 290 | % MasteringMatlab@yahoo.com 291 | % Mastering MATLAB 7 292 | % 2006-11-8 293 | 294 | if nargin==1 % initialize recursive solution 295 | xn=varargin{1}(:); 296 | Pn=diag(1e12+zeros(size(xn))); 297 | 298 | elseif nargin==2 % initialize recursive solution 299 | xn=varargin{1}(:); 300 | if numel(varargin{2})~=1 301 | error('LSREC:scalar','Scalar Weight Required.') 302 | else 303 | W=varargin{2}; 304 | if W<=eps || W>1 305 | error('LSREC:OutofBound','Weight Must be Between 0 and 1.') 306 | end 307 | Pn=diag((1/W)+zeros(size(xn))); 308 | end 309 | 310 | elseif nargin==5 % recursive call 311 | 312 | yn=varargin{1}(:); % make sure yn is a column vector 313 | An=varargin{2}; 314 | Wn=varargin{3}(:); 315 | x=varargin{4}(:); 316 | P=varargin{5}; 317 | if length(yn)~=size(An,1) 318 | error('LSREC:nonconform',... 319 | 'yn Must Have as Many Rows as An.') 320 | end 321 | if size(An,2)~=length(x) 322 | error('LSREC:nonconform',... 323 | 'An Must Have as Many Columns as x has elements.') 324 | end 325 | if size(P,1)~=size(P,2) || size(P,1)~=length(x) 326 | error('LSREC:nonform',... 327 | 'P Must be a Square Matrix of Dimension Equal to length(x).') 328 | end 329 | if length(Wn)~=1 && length(Wn)~=length(yn) 330 | error('LSREC:conform',... 331 | 'Wn Must be a Scalar or Have the Same Number of Elements as yn.') 332 | end 333 | if any(Wn<=eps) || any(Wn>1) 334 | error('LSREC:OutofBound','Weights Must be Between 0 and 1.') 335 | end 336 | if numel(Wn)==1 % expand scalar weight if needed 337 | Wn=repmat(Wn,size(yn)); 338 | end 339 | 340 | K=P*An'/(An*P*An'+diag(1./Wn)); 341 | xn=x+K*(yn-An*x); 342 | if nargout>1 % compute new P 343 | Pn=P-K*An*P; 344 | end 345 | else 346 | error('LSREC:rhs','Recursive Calls Require 5 Input Arguments.') 347 | end 348 | end 349 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 350 | function NFinal = do_RecTri(P,S,W) 351 | Nmat = mlat.RecTrilateration(P,S,W); 352 | N1 = Nmat(:,1:end); 353 | [n m]=size(N1); 354 | for i1=1:m 355 | Nn = N1(:,i1); 356 | Nn = Nn(2:4); 357 | [Sn(i1,:) F(i1)] = mlat.distancen(Nn,P,S); 358 | diff(i1) = N1(1,i1) - norm(Nn).^2; 359 | end 360 | [Fmin Imin] = min(F); 361 | [NrecDel IDel] = min(N1(1,1:3)); 362 | NrecDel = N1(2:4,IDel); 363 | Nrec = N1(:,Imin); 364 | if imag(NrecDel(1))==0 365 | NFinal = NrecDel'; 366 | else 367 | for jj=1:size(N1,2) 368 | if imag(N1(2,jj))==0 369 | NFinal = N1(2:4,jj)'; 370 | end 371 | end 372 | end 373 | end 374 | 375 | %% Normal Trilateration 376 | %%%%%%%%%%%%%%%%% 377 | % Trilateration % 378 | %%%%%%%%%%%%%%%%% 379 | function Nmat = do_Tri(P,S,W) 380 | % paper "An algebraic solution to the multilateration problem" 381 | % Author: Norrdine, Abdelmoumen (norrdine@hotmail.de) 382 | % https://www.researchgate.net/publication/275027725_An_Algebraic_Solution_to_the_Multilateration_Problem 383 | % usage: [N1 N2] = Trilateration(P,S,W) 384 | % P = [P1 P2 P3 P4 ..] Reference points matrix 385 | % S = [s1 s2 s3 s4 ..] distance matrix. 386 | % W : Weights Matrix (Statistics). 387 | % N : calculated solution 388 | % THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY!! 389 | [mp,np] = size(P); 390 | ns = length(S); 391 | if (ns~=np) 392 | error('Number of reference points and distances are different'); 393 | end 394 | A=[]; b=[]; 395 | for i1=1:np 396 | x = P(1,i1); y = P(2,i1); z = P(3,i1); 397 | s = S(i1); 398 | A = [A ; 1 -2*x -2*y -2*z]; 399 | b= [b ; s^2-x^2-y^2-z^2 ]; 400 | end 401 | 402 | if (np==3) 403 | warning off; 404 | Xp= A\b; % Gaussian elimination 405 | % or Xp=pinv(A)*b; 406 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 407 | % depend only on the reference points 408 | % it could be computed only once 409 | xp = Xp(2:4,:); 410 | Z = null(A,'r'); 411 | z = Z(2:4,:); 412 | if rank (A)==3 413 | %Polynom coeff. 414 | a2 = z(1)^2 + z(2)^2 + z(3)^2 ; 415 | a1 = 2*(z(1)*xp(1) + z(2)*xp(2) + z(3)*xp(3))-Z(1); 416 | a0 = xp(1)^2 + xp(2)^2+ xp(3)^2-Xp(1); 417 | p = [a2 a1 a0]; 418 | t = roots(p); 419 | 420 | %Solutions 421 | N1 = Xp + t(1)*Z; 422 | N2 = Xp + t(2)*Z; 423 | if N1(1)3) 431 | %Particular solution 432 | 433 | if W~=diag(ones(1,length(W))) 434 | C = W'*W; 435 | Xpdw =inv(A'*C*A)*A'*inv(C)*b; % Solution with Weights Matrix 436 | else 437 | Xpdw=inv(A'*A)*A'*b; % Solution without Weights Matrix 438 | end 439 | 440 | % the matrix inv(A'*A)*A' or inv(A'*C*A)*A'*C or pinv(A) 441 | % depend only on the reference points 442 | % it could be computed only once 443 | N1 = Xpdw; 444 | N2 = N1; 445 | Nmat = N1(2:4)'; 446 | end 447 | end 448 | end 449 | end --------------------------------------------------------------------------------