├── 3D ├── Airmodel.m ├── All_Variables.m ├── Forces.m ├── Go1.m ├── IMU_meas.m ├── Kalman_X.m ├── Kalman_X2.m ├── Kalman_Y.m ├── Kalman_Y2.m ├── Kalman_Z.m ├── Kalman_Z2.m ├── Kalman_function1.m ├── Kalman_phi.m ├── Kalman_phi2.m ├── Kalman_psi.m ├── Kalman_psi2.m ├── Kalman_theta.m ├── Kalman_theta2.m ├── Kalmanfilter_prob1_sfun.mexw64 ├── Map.m ├── Motors_Speed.m ├── Motors_Speed_2.m ├── PID_X.m ├── PID_X_NC.m ├── PID_Y.m ├── PID_Y_NC.m ├── PID_Z.m ├── PID_Z_NC.m ├── PID_heading.m ├── PID_heading_NC.m ├── PID_pitch.m ├── PID_pitch_NC.m ├── PID_roll.m ├── PID_roll_NC.m ├── Quadrotor.mat ├── XY_meas.m ├── Y_Backstepping.m ├── Z_meas.m ├── copter_model.m ├── copter_model_3D.m ├── example.m ├── phi_meas.m ├── plot_X.m ├── plot_XY.m ├── plot_Y.m ├── plot_Z.m ├── plot_forces.m ├── plot_motors.m ├── plot_phi.m ├── plot_psi.m ├── plot_quad.m ├── plot_quad_3D.m ├── plot_theta.m ├── quadmodel.m ├── rotateXYZ.m ├── rotateXYZ2.m └── script2run.m ├── 3D_with_LIDAR ├── 3D + LIDAR │ ├── 1.jpg │ ├── Airmodel.m │ ├── All_Variables.m │ ├── Forces.m │ ├── Go1.m │ ├── IMU_meas.m │ ├── Kalman.jpg │ ├── Kalman_X.m │ ├── Kalman_X2.m │ ├── Kalman_Y.m │ ├── Kalman_Y2.m │ ├── Kalman_Z.m │ ├── Kalman_Z2.m │ ├── Kalman_function1.m │ ├── Kalman_phi.m │ ├── Kalman_phi2.m │ ├── Kalman_psi.m │ ├── Kalman_psi2.m │ ├── Kalman_theta.m │ ├── Kalman_theta2.m │ ├── Kalmanfilter_prob1_sfun.mexw64 │ ├── LIDAR_Plot.m │ ├── Map.m │ ├── Motors_Speed.m │ ├── Motors_Speed_2.m │ ├── Obstacles_X12_Y12_Z12.fig │ ├── Obstacles_X12_Y12_Z12.png │ ├── PID_Roll.jpg │ ├── PID_X.m │ ├── PID_X_NC.m │ ├── PID_Y.m │ ├── PID_Y_NC.m │ ├── PID_Z.m │ ├── PID_Z_NC.m │ ├── PID_heading.m │ ├── PID_heading_NC.m │ ├── PID_pitch.m │ ├── PID_pitch_NC.m │ ├── PID_roll.m │ ├── PID_roll_NC.m │ ├── Path_manager.m │ ├── Quadrotor.mat │ ├── Quadrotor_sfun.mexw64 │ ├── READ ME.txt │ ├── SimInitial.m │ ├── SimInitial2.m │ ├── Sim_LIDAR.m │ ├── Sim_LIDAR_2.m │ ├── XY_meas.m │ ├── Y_Backstepping.m │ ├── Z_meas.m │ ├── copter_model.m │ ├── copter_model_3D.m │ ├── example.m │ ├── obstacle.m │ ├── obstacles.png │ ├── phi_meas.m │ ├── plot_X.m │ ├── plot_XY.m │ ├── plot_Y.m │ ├── plot_Z.m │ ├── plot_forces.m │ ├── plot_motors.m │ ├── plot_phi.m │ ├── plot_psi.m │ ├── plot_quad.m │ ├── plot_quad_3D.m │ ├── plot_theta.m │ ├── quadmodel.m │ ├── rotateXYZ.m │ ├── script2run.m │ ├── script2run2.m │ ├── simLIDARinitial.m │ └── slprj │ │ └── _sfprj │ │ └── Quadrotor │ │ └── _self │ │ └── sfun │ │ ├── info │ │ ├── binfo.mat │ │ ├── chart1_ZfxxQE31IEuUdTwkikIDi.mat │ │ ├── chart2_9mJDJgrA6frDphDvY5VxDD.mat │ │ └── chart3_01Yt7r9mvv6kgKbp23oyGC.mat │ │ └── src │ │ ├── Quadrotor_sfun.bat │ │ ├── Quadrotor_sfun.c │ │ ├── Quadrotor_sfun.exp │ │ ├── Quadrotor_sfun.h │ │ ├── Quadrotor_sfun.lib │ │ ├── Quadrotor_sfun.mak │ │ ├── Quadrotor_sfun.map │ │ ├── Quadrotor_sfun.mexw64.manifest │ │ ├── Quadrotor_sfun.mol │ │ ├── Quadrotor_sfun.obj │ │ ├── Quadrotor_sfun_debug_macros.h │ │ ├── Quadrotor_sfun_registry.c │ │ ├── Quadrotor_sfun_registry.obj │ │ ├── c1_Quadrotor.c │ │ ├── c1_Quadrotor.h │ │ ├── c1_Quadrotor.obj │ │ ├── c2_Quadrotor.c │ │ ├── c2_Quadrotor.h │ │ ├── c2_Quadrotor.obj │ │ ├── c3_Quadrotor.c │ │ ├── c3_Quadrotor.h │ │ ├── c3_Quadrotor.obj │ │ ├── mexopts.bat │ │ ├── rtwtypes.h │ │ └── rtwtypeschksum.mat └── 3D+LIDAR+path │ ├── 1.jpg │ ├── Airmodel.m │ ├── All_Variables.m │ ├── Forces.m │ ├── Go1.m │ ├── IMU_meas.m │ ├── Kalman.jpg │ ├── Kalman_X.m │ ├── Kalman_X2.m │ ├── Kalman_Y.m │ ├── Kalman_Y2.m │ ├── Kalman_Z.m │ ├── Kalman_Z2.m │ ├── Kalman_function1.m │ ├── Kalman_phi.m │ ├── Kalman_phi2.m │ ├── Kalman_psi.m │ ├── Kalman_psi2.m │ ├── Kalman_theta.m │ ├── Kalman_theta2.m │ ├── Kalmanfilter_prob1_sfun.mexw64 │ ├── LIDAR_Plot.m │ ├── Map.m │ ├── Motors_Speed.m │ ├── Motors_Speed_2.m │ ├── Obstacles_X12_Y12_Z12.fig │ ├── Obstacles_X12_Y12_Z12.png │ ├── PID_Roll.jpg │ ├── PID_X.m │ ├── PID_X_NC.m │ ├── PID_Y.m │ ├── PID_Y_NC.m │ ├── PID_Z.m │ ├── PID_Z_NC.m │ ├── PID_heading.m │ ├── PID_heading_NC.m │ ├── PID_pitch.m │ ├── PID_pitch_NC.m │ ├── PID_roll.m │ ├── PID_roll_NC.m │ ├── Path_manager.m │ ├── Quadrotor.mat │ ├── Quadrotor_sfun.mexw64 │ ├── READ ME.txt │ ├── SimInitial.m │ ├── SimInitial2.m │ ├── Sim_LIDAR.m │ ├── Sim_LIDAR_2.m │ ├── XY_meas.m │ ├── Y_Backstepping.m │ ├── Z_meas.m │ ├── copter_model.m │ ├── copter_model_3D.m │ ├── example.m │ ├── motor.jpg │ ├── motor2.jpg │ ├── obstacle.m │ ├── obstacles.png │ ├── path.jpg │ ├── phi1.jpg │ ├── phi_meas.m │ ├── plot_X.m │ ├── plot_XY.m │ ├── plot_Y.m │ ├── plot_Z.m │ ├── plot_forces.m │ ├── plot_motors.m │ ├── plot_phi.m │ ├── plot_psi.m │ ├── plot_quad.m │ ├── plot_quad_3D.m │ ├── plot_theta.m │ ├── psi1.jpg │ ├── quadmodel.m │ ├── rotateXYZ.m │ ├── script2run.m │ ├── script2run2.m │ ├── simLIDARinitial.m │ ├── slprj │ └── _sfprj │ │ └── Quadrotor │ │ └── _self │ │ └── sfun │ │ ├── info │ │ ├── binfo.mat │ │ ├── chart1_ZfxxQE31IEuUdTwkikIDi.mat │ │ ├── chart2_9mJDJgrA6frDphDvY5VxDD.mat │ │ └── chart3_01Yt7r9mvv6kgKbp23oyGC.mat │ │ └── src │ │ ├── Quadrotor_sfun.bat │ │ ├── Quadrotor_sfun.c │ │ ├── Quadrotor_sfun.exp │ │ ├── Quadrotor_sfun.h │ │ ├── Quadrotor_sfun.lib │ │ ├── Quadrotor_sfun.mak │ │ ├── Quadrotor_sfun.map │ │ ├── Quadrotor_sfun.mexw64.manifest │ │ ├── Quadrotor_sfun.mol │ │ ├── Quadrotor_sfun.obj │ │ ├── Quadrotor_sfun_debug_macros.h │ │ ├── Quadrotor_sfun_registry.c │ │ ├── Quadrotor_sfun_registry.obj │ │ ├── c1_Quadrotor.c │ │ ├── c1_Quadrotor.h │ │ ├── c1_Quadrotor.obj │ │ ├── c2_Quadrotor.c │ │ ├── c2_Quadrotor.h │ │ ├── c2_Quadrotor.obj │ │ ├── c3_Quadrotor.c │ │ ├── c3_Quadrotor.h │ │ ├── c3_Quadrotor.obj │ │ ├── mexopts.bat │ │ ├── rtwtypes.h │ │ └── rtwtypeschksum.mat │ ├── theta1.jpg │ ├── x.jpg │ ├── x1.jpg │ ├── x2.jpg │ ├── y.jpg │ ├── y1.jpg │ ├── y2.jpg │ └── z2.jpg ├── Integrated Simulation Platform for Indoor Quadrotor Applications.pdf ├── LICENSE ├── README.md └── doc ├── quadrotor1.png └── quadrotor2.png /3D/Airmodel.m: -------------------------------------------------------------------------------- 1 | global A 2 | -------------------------------------------------------------------------------- /3D/Forces.m: -------------------------------------------------------------------------------- 1 | function Forces 2 | global A 3 | % this function will calculates the forces and torques on the quadrotor 4 | % give the speed of each propeler. 5 | 6 | % Inputs 7 | % O1 = Front (CCW) 8 | % O2 = Right (CW) 9 | % O3 = Rear (CCW) 10 | % O4 = Left (CW) 11 | 12 | % Outputs 13 | % U1 = Throttle 14 | % U2 = Roll movement 15 | % U3 = Pitch movement 16 | % U4 = Yaw movement 17 | 18 | % Constants 19 | % l = Where l [m] is the distance between the center of the quadrotor and 20 | % the center of a propeller. 21 | % b = 22 | 23 | % A.U11_plot(A.counter) = A.U1; 24 | A.U1 = A.b*(sign(A.O1)*A.O1^2 + sign(A.O2)*A.O2^2 + sign(A.O3)*A.O3^2 + sign(A.O4)*A.O4^2); 25 | A.U1_plot(A.counter) = A.U1; 26 | 27 | % A.U21_plot(A.counter) = A.U1; 28 | A.U2 = A.b*A.l*(sign(A.O4)*A.O4^2 - sign(A.O2)*A.O2^2); 29 | A.U2_plot(A.counter) = A.U2; 30 | 31 | % A.U31_plot(A.counter) = A.U1; 32 | A.U3 = A.b*A.l*(sign(A.O3)*A.O3^2 - sign(A.O1)*A.O1^2); 33 | A.U3_plot(A.counter) = A.U3; 34 | 35 | % A.U41_plot(A.counter) = A.U1; 36 | A.U4 = A.d*(sign(A.O2)*A.O2^2 + sign(A.O4)*A.O4^2 - sign(A.O1)*A.O1^2 - sign(A.O3)*A.O3^2); 37 | A.U4_plot(A.counter) = A.U4; 38 | 39 | A.O = (-A.O1 + A.O2 - A.O3 + A.O4); 40 | A.O_plot(A.counter) = A.O; 41 | 42 | 43 | end -------------------------------------------------------------------------------- /3D/Go1.m: -------------------------------------------------------------------------------- 1 | global A -------------------------------------------------------------------------------- /3D/IMU_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter); %erros is +- 2 mrad 5 | A.theta_meas = A.theta+A.theta_error(A.counter); %erros is +- 2 mrad 6 | A.psi_meas = A.psi+A.psi_error(A.counter); %erros is +- 2 mrad 7 | 8 | end -------------------------------------------------------------------------------- /3D/Kalman_X.m: -------------------------------------------------------------------------------- 1 | function Kalman_X 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (A.U1/A.m*A.theta_kalman - (A.X_dot*cos(A.psi_kalman)+A.Y_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_X2.m: -------------------------------------------------------------------------------- 1 | function Kalman_X2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((sin(A.psi_kalman)*sin(A.phi_kalman) + cos(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.X_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_Y.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.U1/A.m*A.phi_kalman - (A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | A.Y_kalman 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_Y2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((-cos(A.psi_kalman)*sin(A.phi_kalman) + sin(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.Y_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_Z.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_Z2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + (cos(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_function1.m: -------------------------------------------------------------------------------- 1 | function xhat = Kalman_function1(z,xpp,RR,QQ) 2 | persistent Pp Q R xp A H I 3 | if isempty(Pp) 4 | Pp=1; Q=QQ; R=RR; 5 | xp=xpp; A=0.5; H=1; I=eye(1); 6 | end 7 | % Compute priori 8 | xp=A*xp; 9 | Pp=A*Pp*A'+Q; 10 | % Measurement update 11 | K = Pp*H'*inv(H*Pp*H'+R); 12 | xhat = xp+K*(z-H*xp); 13 | % Update covariance 14 | Pp=(I-K*H)*Pp; 15 | xp = xhat; 16 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_phi.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.p*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_phi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(A.p + sin(A.phi_kalman)*tan(A.theta_kalman)*A.q + cos(A.phi_kalman)*tan(A.theta_kalman)*A.r)*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_psi.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.r*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_psi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(sin(A.phi_kalman)/cos(A.theta_kalman)*A.q + cos(A.phi_kalman)/cos(A.theta_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_theta.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.q*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D/Kalman_theta2.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(cos(A.phi_kalman)*A.q - sin(A.phi_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D/Kalmanfilter_prob1_sfun.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D/Kalmanfilter_prob1_sfun.mexw64 -------------------------------------------------------------------------------- /3D/Map.m: -------------------------------------------------------------------------------- 1 | global A 2 | % imshow('AUS.jpg') 3 | A.aus = imread('AUS.jpg'); 4 | axis equal 5 | % [x,y,z] = sphere(50); 6 | A.t1=-250:1:249; 7 | A.c1=-250:1:249; 8 | A.x1=[]; 9 | A.y1=[]; 10 | A.z1=zeros(500,500); 11 | A.b=0; 12 | while A.b<500; 13 | A.x1=[A.x1;A.c1]; 14 | A.y1=[A.y1;A.t1]; 15 | A.b = A.b+1; 16 | end 17 | A.y1=A.y1'; 18 | A.props.AmbientStrength = 0.1; 19 | A.props.DiffuseStrength = 1; 20 | A.props.SpecularColorReflectance = .5; 21 | A.props.SpecularExponent = 20; 22 | A.props.SpecularStrength = 1; 23 | A.props.FaceColor= 'texture'; 24 | A.props.EdgeColor = 'none'; 25 | A.props.FaceLighting = 'phong'; 26 | A.props.Cdata = A.aus; 27 | surface(A.x1,A.y1,A.z1,A.props); -------------------------------------------------------------------------------- /3D/Motors_Speed.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if abs(BB1)>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if abs(BB2)>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if abs(BB3)>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if abs(BB4)>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | A.O1 = sign(BB1)*sqrt(abs(BB1)); % Front M 26 | A.O2 = sign(BB2)*sqrt(abs(BB2)); % Right M 27 | A.O3 = sign(BB3)*sqrt(abs(BB3)); % Rear M 28 | A.O4 = sign(BB4)*sqrt(abs(BB4)); % Left M 29 | 30 | A.O1_plot(A.counter) = A.O1; 31 | A.O2_plot(A.counter) = A.O2; 32 | A.O3_plot(A.counter) = A.O3; 33 | A.O4_plot(A.counter) = A.O4; 34 | 35 | end -------------------------------------------------------------------------------- /3D/Motors_Speed_2.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed_2 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if BB1>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if BB2>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if BB3>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if BB4>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | if BB1 pi/4) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/4; 13 | end 14 | -------------------------------------------------------------------------------- /3D/PID_X_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.X_des = A.X_des_EF*cos(A.psi_meas)+A.Y_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | 5 | A.X_BF = A.X_meas*cos(A.psi_meas)+A.Y_meas*sin(A.psi_meas); % calculating X in BF 6 | A.X_dot_BF = A.X_dot*cos(A.psi_meas)+A.Y_dot*sin(A.psi_meas); % calculating X_dot in BF 7 | 8 | % PD controller for X_position 9 | A.theta_des = A.X_KP*(A.X_des - A.X_BF) + A.X_KD*A.X_dot; 10 | 11 | if(abs(A.theta_des) > pi/18) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/18; 13 | end 14 | -------------------------------------------------------------------------------- /3D/PID_Y.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_kalman)-A.X_des_EF*sin(A.psi_kalman); % calculating the desired X in BF 4 | A.Y_BF = A.Y_kalman*cos(A.psi_kalman)-A.X_kalman*sin(A.psi_kalman); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/4) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/4; 12 | end 13 | -------------------------------------------------------------------------------- /3D/PID_Y_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_meas)-A.X_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | A.Y_BF = A.Y_meas*cos(A.psi_meas)-A.X_meas*sin(A.psi_meas); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_meas)-A.X_dot*sin(A.psi_meas); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/18) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/18; 12 | end 13 | -------------------------------------------------------------------------------- /3D/PID_Z.m: -------------------------------------------------------------------------------- 1 | function PID_Z 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_kalman; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_kalman)/cos(A.theta_kalman); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /3D/PID_Z_NC.m: -------------------------------------------------------------------------------- 1 | function PID_Z_NC 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_meas; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_meas)/cos(A.theta_meas); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /3D/PID_heading.m: -------------------------------------------------------------------------------- 1 | function PID_heading 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_kalman ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D/PID_heading_NC.m: -------------------------------------------------------------------------------- 1 | function PID_heading_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_meas ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D/PID_pitch.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_kalman; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /3D/PID_pitch_NC.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch_NC 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_meas; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /3D/PID_roll.m: -------------------------------------------------------------------------------- 1 | function PID_roll 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_kalman; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D/PID_roll_NC.m: -------------------------------------------------------------------------------- 1 | function PID_roll_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_meas; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D/Quadrotor.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D/Quadrotor.mat -------------------------------------------------------------------------------- /3D/XY_meas.m: -------------------------------------------------------------------------------- 1 | function XY_meas 2 | global A 3 | 4 | A.X_meas = A.X+A.X_error(A.counter); %erros is +- 1 cm 5 | A.Y_meas = A.Y+A.Y_error(A.counter); %erros is +- 1 cm 6 | end -------------------------------------------------------------------------------- /3D/Y_Backstepping.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | if(A.U1~=0) 4 | A.U2 = .1*(-5*(A.Y - A.Y_des) -10*A.Y_dot - 9*A.U1*A.S_psi*A.C_phi ... 5 | -4*A.U1*A.psi_dot*A.C_psi*A.C_phi + A.U1*A.psi_dot^2*A.S_psi*A.C_phi ... 6 | +2*A.U1*A.psi_dot*A.S_psi*A.S_phi + A.U1*A.psi_dot*A.phi_dot*A.C_psi*A.S_phi ... 7 | -A.U1*A.phi_dot*A.psi_dot*A.C_psi*A.S_phi - A.U1*A.phi_dot^2*A.S_psi*A.C_phi)/(A.U1*A.C_psi*A.C_phi); 8 | else 9 | A.U2 = 0; 10 | end -------------------------------------------------------------------------------- /3D/Z_meas.m: -------------------------------------------------------------------------------- 1 | function Z_meas 2 | global A 3 | 4 | A.Z_meas = A.Z+A.Z_error(A.counter); %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /3D/copter_model.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | x1=-.27; 4 | x2=.27; 5 | y1=-.01; 6 | y2=.01; 7 | z1=-.01; 8 | z2=.01; 9 | 10 | acon=[x1 y1 z1; 11 | x2 y1 z1; 12 | x2 y2 z1; 13 | x1 y2 z1; 14 | x1 y1 z2; 15 | x2 y1 z2; 16 | x2 y2 z2; 17 | x1 y2 z2]; 18 | 19 | bcon=[1 2 6 5; 20 | 2 3 7 6; 21 | 3 4 8 7; 22 | 4 1 5 8; 23 | 1 2 3 4; 24 | 5 6 7 8]; 25 | 26 | 27 | body1=patch('faces',bcon,... 28 | 'vertices',acon,... 29 | 'facecolor',[.8 .8 .8],... 30 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 31 | 32 | y1=-.27; 33 | y2=.27; 34 | x1=-.01; 35 | x2=.01; 36 | z1=-.01; 37 | z2=.01; 38 | 39 | acon=[x1 y1 z1; 40 | x2 y1 z1; 41 | x2 y2 z1; 42 | x1 y2 z1; 43 | x1 y1 z2; 44 | x2 y1 z2; 45 | x2 y2 z2; 46 | x1 y2 z2]; 47 | 48 | body2=patch('faces',bcon,... 49 | 'vertices',acon,... 50 | 'facecolor',[.8 .8 .8],... 51 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 52 | 53 | t=0:pi/10:2*pi; 54 | X=.13*cos(t); 55 | Y=.13*sin(t); 56 | Z=zeros(size(t))+.015; 57 | C=zeros(size(t)); 58 | A.C = C; 59 | 60 | RotorF = patch(X+.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 61 | RotorB = patch(X-.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 62 | RotorL = patch(X,Y+.27,Z,C,'facealpha',.3,'facecolor','k'); 63 | RotorR = patch(X,Y-.27,Z,C,'facealpha',.3,'facecolor','k'); 64 | 65 | A.Body1X = get(body1,'xdata'); 66 | A.Body1Y = get(body1,'ydata'); 67 | A.Body1Z = get(body1,'zdata'); 68 | 69 | A.Body2X = get(body2,'xdata'); 70 | A.Body2Y = get(body2,'ydata'); 71 | A.Body2Z = get(body2,'zdata'); 72 | 73 | A.RotorFX = get(RotorF,'xdata'); 74 | A.RotorFY = get(RotorF,'ydata'); 75 | A.RotorFZ = get(RotorF,'zdata'); 76 | 77 | A.RotorBX = get(RotorB,'xdata'); 78 | A.RotorBY = get(RotorB,'ydata'); 79 | A.RotorBZ = get(RotorB,'zdata'); 80 | 81 | A.RotorRX = get(RotorR,'xdata'); 82 | A.RotorRY = get(RotorR,'ydata'); 83 | A.RotorRZ = get(RotorR,'zdata'); 84 | 85 | A.RotorLX = get(RotorL,'xdata'); 86 | A.RotorLY = get(RotorL,'ydata'); 87 | A.RotorLZ = get(RotorL,'zdata'); 88 | 89 | % xlabel('x') 90 | % grid on 91 | % axis([-1 1 -1 1 -1 1]); 92 | axis equal 93 | 94 | -------------------------------------------------------------------------------- /3D/copter_model_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | load Quadrotor 4 | 5 | A.Body1 = patch('xdata',A.Body1X,'ydata',A.Body1Y,'zdata',A.Body1Z,'facealpha',.3,'facecolor','b'); 6 | A.Body2 = patch('xdata',A.Body2X,'ydata',A.Body2Y,'zdata',A.Body2Z,'facealpha',.3,'facecolor','b'); 7 | A.RotorF = patch('xdata',A.RotorFX,'ydata',A.RotorFY,'zdata',A.RotorFZ,'facealpha',.3,'facecolor','k'); 8 | A.RotorB = patch('xdata',A.RotorBX,'ydata',A.RotorBY,'zdata',A.RotorBZ,'facealpha',.3,'facecolor','k'); 9 | A.RotorR = patch('xdata',A.RotorRX,'ydata',A.RotorRY,'zdata',A.RotorRZ,'facealpha',.3,'facecolor','k'); 10 | A.RotorL = patch('xdata',A.RotorLX,'ydata',A.RotorLY,'zdata',A.RotorLZ,'facealpha',.3,'facecolor','k'); 11 | 12 | -------------------------------------------------------------------------------- /3D/example.m: -------------------------------------------------------------------------------- 1 | load wind 2 | wind_speed = sqrt(u.^2 + v.^2 + w.^2); 3 | 4 | % hpatch = patch(isosurface(x,y,z,wind_speed,35)); 5 | % isonormals(x,y,z,wind_speed,hpatch) 6 | % set(hpatch,'FaceColor','red','EdgeColor','none'); 7 | % 8 | % [f vt] = reducepatch(isosurface(x,y,z,wind_speed,45),0.05); 9 | % daspect([1,1,1]); 10 | % hcone = coneplot(x,y,z,u,v,w,vt(:,1),vt(:,2),vt(:,3),2); 11 | % set(hcone,'FaceColor','blue','EdgeColor','none'); 12 | 13 | camproj perspective 14 | camva(25) 15 | 16 | hlight = camlight('headlight'); 17 | % set(hpatch,'AmbientStrength',.1,... 18 | % 'SpecularStrength',1,... 19 | % 'DiffuseStrength',1); 20 | % set(hcone,'SpecularStrength',1); 21 | % set(gcf,'Color','k') 22 | 23 | lighting gouraud 24 | set(gcf,'Renderer','OpenGL') 25 | 26 | hsline = streamline(x,y,z,u,v,w,80,30,11); 27 | xd = get(hsline,'XData'); 28 | yd = get(hsline,'YData'); 29 | zd = get(hsline,'ZData'); 30 | delete(hsline) 31 | tic 32 | for i=1:length(xd)-50 33 | campos([xd(i),yd(i),zd(i)]) 34 | camtarget([xd(i+5)+min(xd)/100,yd(i),zd(i)]) 35 | camlight(hlight,'headlight') 36 | drawnow 37 | toc 38 | end -------------------------------------------------------------------------------- /3D/phi_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter) %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /3D/plot_X.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter)+A.X_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.X_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.X_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.X_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /3D/plot_XY.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot3(A.X_plot(1:A.counter-1),A.Y_plot(1:A.counter-1),A.Z_plot(1:A.counter-1)) 7 | view(30,30) 8 | grid on 9 | axis equal 10 | axis([min(A.X_plot)-.2 max(A.X_plot)+.2 min(A.Y_plot)-.2 max(A.Y_plot)+.2 min(A.Z_plot)-.2 max(A.Z_plot)+.2]) 11 | % plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter),'r','linewidth',1) 12 | % plot(A.t_plot(1:A.counter),A.Y_ref_plot(1:A.counter),'b') 13 | % plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 14 | legend('Actual path') 15 | 16 | xlabel('X axis (m)') 17 | ylabel('Y axis (m)') 18 | zlabel('Z axis (m)') 19 | 20 | title('path traveled') -------------------------------------------------------------------------------- /3D/plot_Y.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter)+A.Y_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.Y_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.Y_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /3D/plot_Z.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter)+A.Z_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.Z_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /3D/plot_forces.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | plot(A.t_plot(1:A.counter-1),A.U1_plot(1:A.counter-1),'r') 6 | plot(A.t_plot(1:A.counter-1),A.U2_plot(1:A.counter-1),'g') 7 | plot(A.t_plot(1:A.counter-1),A.U3_plot(1:A.counter-1),'b') 8 | plot(A.t_plot(1:A.counter-1),A.U4_plot(1:A.counter-1),'k') 9 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 11 | legend('Throttle','Roll','Pitch','Yaw') 12 | xlabel('time (s)') 13 | ylabel('value') 14 | title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /3D/plot_motors.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.O1_plot(1:A.counter),'r') 6 | plot(A.t_plot(1:A.counter),A.O2_plot(1:A.counter),'g') 7 | plot(A.t_plot(1:A.counter),A.O3_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.O4_plot(1:A.counter),'k') 9 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 11 | legend('front','right','rear','left') 12 | xlabel('time (s)') 13 | ylabel('value') 14 | title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /3D/plot_phi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter)+A.phi_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.phi_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.phi_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.phi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('phi (rad)') 13 | title('phi vs. time') -------------------------------------------------------------------------------- /3D/plot_psi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter)+A.psi_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.psi_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.psi_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.psi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('psi (rad)') 13 | title('psi vs. time') -------------------------------------------------------------------------------- /3D/plot_quad.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X2,A.Y2,A.Z2,A.phi,A.theta,A.psi); 4 | set(A.L2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.FPX,A.FPY,A.FPZ,A.phi,A.theta,A.psi); 7 | set(A.L3,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.RPX,A.RPY,A.RPZ,A.phi,A.theta,A.psi); 10 | set(A.L4,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.rPX,A.rPY,A.rPZ,A.phi,A.theta,A.psi); 13 | set(A.L5,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.LPX,A.LPY,A.LPZ,A.phi,A.theta,A.psi); 16 | set(A.L6,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X1,A.Y1,A.Z1,A.phi,A.theta,A.psi); 19 | set(A.L1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 20 | -------------------------------------------------------------------------------- /3D/plot_quad_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body1X,A.Body1Y,A.Body1Z,A.phi,A.theta,A.psi); 4 | set(A.Body1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body2X,A.Body2Y,A.Body2Z,A.phi,A.theta,A.psi); 7 | set(A.Body2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorFX,A.RotorFY,A.RotorFZ,A.phi,A.theta,A.psi); 10 | set(A.RotorF,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorBX,A.RotorBY,A.RotorBZ,A.phi,A.theta,A.psi); 13 | set(A.RotorB,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorLX,A.RotorLY,A.RotorLZ,A.phi,A.theta,A.psi); 16 | set(A.RotorL,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorRX,A.RotorRY,A.RotorRZ,A.phi,A.theta,A.psi); 19 | set(A.RotorR,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z) -------------------------------------------------------------------------------- /3D/plot_theta.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter)+A.theta_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.theta_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.theta_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.theta_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('theta (rad)') 13 | title('theta vs. time') -------------------------------------------------------------------------------- /3D/quadmodel.m: -------------------------------------------------------------------------------- 1 | function quadmodel 2 | global A 3 | % this function simulate the dynamics of the quadrotor, it will take the 4 | % Euler angels and forces as inputs and give the linear accelerations WRT 5 | % E-Frame and rotational accelerations WRT B-Frame. 6 | 7 | A.X_ddot = (sin(A.psi)*sin(A.phi) + cos(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 8 | A.Y_ddot = (-cos(A.psi)*sin(A.phi) + sin(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 9 | A.Z_ddot = -A.g + (cos(A.theta)*cos(A.phi))*A.U1/A.m; 10 | 11 | A.p_dot = ((A.Iyy - A.Izz)*A.q*A.r -A.Jtp*A.q*A.O + A.U2)/A.Ixx; 12 | A.q_dot = ((A.Izz - A.Ixx)*A.p*A.r +A.Jtp*A.p*A.O + A.U3)/A.Iyy; 13 | A.r_dot = ((A.Ixx - A.Iyy)*A.p*A.q + A.U4)/A.Izz; 14 | 15 | A.phi_dot = A.p + sin(A.phi)*tan(A.theta)*A.q + cos(A.phi)*tan(A.theta)*A.r; 16 | A.theta_dot = cos(A.phi)*A.q - sin(A.phi)*A.r; 17 | A.psi_dot = sin(A.phi)/cos(A.theta)*A.q + cos(A.phi)/cos(A.theta)*A.r; 18 | 19 | % Air friction model 20 | A.X_ddot = A.X_ddot - A.X_dot*1.2; 21 | A.Y_ddot = A.Y_ddot - A.Y_dot*1.2; 22 | 23 | % Disturbance model 24 | A.Z_ddot = A.Z_ddot + A.Z_dis/A.m; 25 | 26 | A.psi_dot = A.psi_dot + A.psi_dis/A.Izz*A.Ts; 27 | 28 | A.theta_dot = A.theta_dot + A.theta_dis/A.Iyy*A.Ts; 29 | 30 | A.phi_dot = A.phi_dot + A.phi_dis/A.Ixx*A.Ts; 31 | 32 | 33 | % Calculating the Z velocity & position 34 | A.Z_dot = A.Z_ddot*A.Ts + A.Z_dot; 35 | A.Z = A.Z_dot*A.Ts + A.Z; 36 | 37 | % Calculating the X velocity & position 38 | A.X_dot = A.X_ddot*A.Ts + A.X_dot; 39 | A.X = A.X_dot*A.Ts + A.X; 40 | 41 | % Calculating the Y velocity & position 42 | A.Y_dot = A.Y_ddot*A.Ts + A.Y_dot; 43 | A.Y = A.Y_dot*A.Ts + A.Y; 44 | 45 | % Calculating p,q,r 46 | A.p = A.p_dot*A.Ts+A.p; 47 | A.q = A.q_dot*A.Ts+A.q; 48 | A.r = A.r_dot*A.Ts+A.r; 49 | 50 | % Calculating phi,theta,psi 51 | A.phi = A.phi_dot*A.Ts+A.phi; 52 | A.theta = A.theta_dot*A.Ts+A.theta; 53 | A.psi = A.psi_dot*A.Ts+A.psi; 54 | 55 | % Plotting variables 56 | A.Z_plot(A.counter) = A.Z; 57 | A.Z_ref_plot(A.counter) = A.Z_des; 58 | A.Z_dis_plot(A.counter) = A.Z_dis; 59 | 60 | A.X_plot(A.counter) = A.X; 61 | A.X_ref_plot(A.counter) = A.X_des; 62 | A.X_dis_plot(A.counter) = A.X_dis; 63 | 64 | A.Y_plot(A.counter) = A.Y; 65 | A.Y_ref_plot(A.counter) = A.Y_des; 66 | A.Y_dis_plot(A.counter) = A.Y_dis; 67 | 68 | A.phi_plot(A.counter) = A.phi; 69 | A.phi_ref_plot(A.counter) = A.phi_des; 70 | A.phi_dis_plot(A.counter) = A.phi_dis; 71 | 72 | A.theta_plot(A.counter) = A.theta; 73 | A.theta_ref_plot(A.counter) = A.theta_des; 74 | A.theta_dis_plot(A.counter) = A.theta_dis; 75 | 76 | A.psi_plot(A.counter) = A.psi; 77 | A.psi_ref_plot(A.counter) = A.psi_des; 78 | A.psi_dis_plot(A.counter) = A.psi_dis; 79 | 80 | A.counter = A.counter + 1; 81 | 82 | % Disturbance removal 83 | if(A.Z_dis ~= 0) 84 | A.Z_dis = 0; 85 | end 86 | 87 | if(A.psi_dis ~= 0) 88 | A.psi_dis = 0; 89 | end 90 | 91 | if(A.theta_dis ~= 0) 92 | A.theta_dis = 0; 93 | end 94 | 95 | if(A.phi_dis ~= 0) 96 | A.phi_dis = 0; 97 | end 98 | 99 | end -------------------------------------------------------------------------------- /3D/rotateXYZ.m: -------------------------------------------------------------------------------- 1 | function [X,Y,Z]=rotateXYZ(X,Y,Z,phi,theta,psi) 2 | % define rotation matrix 3 | R_roll = [... 4 | 1, 0, 0;... 5 | 0, cos(phi), -sin(phi);... 6 | 0, sin(phi), cos(phi)]; 7 | R_pitch = [... 8 | cos(theta), 0, sin(theta);... 9 | 0, 1, 0;... 10 | -sin(theta), 0, cos(theta)]; 11 | R_yaw = [... 12 | cos(psi), -sin(psi), 0;... 13 | sin(psi), cos(psi), 0;... 14 | 0, 0, 1]; 15 | R = R_roll'*R_pitch'*R_yaw'; 16 | 17 | % rotate vertices 18 | for i=1:length(X) 19 | pts = [X(i), Y(i), Z(i)]*R; 20 | 21 | X(i) = pts(:,1); 22 | Y(i) = pts(:,2); 23 | Z(i) = pts(:,3); 24 | end 25 | end -------------------------------------------------------------------------------- /3D/rotateXYZ2.m: -------------------------------------------------------------------------------- 1 | function [X,Y,Z]=rotateXYZ2(X,Y,Z,phi,theta,psi) 2 | % define rotation matrix 3 | R_roll = [... 4 | 1, 0, 0;... 5 | 0, cos(phi), -sin(phi);... 6 | 0, sin(phi), cos(phi)]; 7 | R_pitch = [... 8 | cos(theta), 0, sin(theta);... 9 | 0, 1, 0;... 10 | -sin(theta), 0, cos(theta)]; 11 | R_yaw = [... 12 | cos(psi), -sin(psi), 0;... 13 | sin(psi), cos(psi), 0;... 14 | 0, 0, 1]; 15 | R = R_roll'*R_pitch'*R_yaw'; 16 | 17 | % rotate vertices 18 | B=size(X); 19 | 20 | for i=1:B(2)*B(1) 21 | pts = [X(i), Y(i), Z(i)]*R; 22 | 23 | X(i) = pts(:,1); 24 | Y(i) = pts(:,2); 25 | Z(i) = pts(:,3); 26 | end 27 | end -------------------------------------------------------------------------------- /3D/script2run.m: -------------------------------------------------------------------------------- 1 | function script2run 2 | 3 | 4 | 5 | %-----------------------------------------------------------------------% 6 | % % 7 | % This script simulates a quadrotor. % 8 | % % 9 | % Done By: Muhannad A.R. Al-Omari % 10 | % Supervised by : Dr. Mohammad Jaradat % 11 | % American University of Sharjah (AUS) % 12 | % % 13 | %-----------------------------------------------------------------------% 14 | 15 | clear all 16 | clc 17 | 18 | global A 19 | 20 | figure('units','normalized','position',[.1 .1 .8 .8],'name','Quadrotor AUS','numbertitle','off','color','w'); 21 | axes('units','normalized','position',[.2 .1 .6 .8]); 22 | % subplot(2,1,1) 23 | axis equal 24 | 25 | % subplot('position',[0.01, 0.01, 0.6, 0.6]); 26 | % axis([-1,5,-3,5,-.1, 2]); 27 | % set(gca,'visible','on') 28 | 29 | All_Variables; 30 | 31 | quadmodel; 32 | % Map; 33 | E1 = uicontrol('units','normalized','position',[.11 .85 .1 .07],'style','edit','fontsize',13,'string',0,'backgroundcolor','w'); 34 | % E2 = uicontrol('units','normalized','position',[.11 .75 .1 .07],'style','edit','fontsize',13,'string',0,'backgroundcolor','w'); 35 | % E3 = uicontrol('units','normalized','position',[.11 .65 .1 .07],'style','edit','fontsize',13,'string',0,'backgroundcolor','w'); 36 | E4 = uicontrol('units','normalized','position',[.11 .55 .1 .07],'style','edit','fontsize',13,'string',0,'backgroundcolor','w'); 37 | E5 = uicontrol('units','normalized','position',[.11 .45 .1 .07],'style','edit','fontsize',13,'string',0,'backgroundcolor','w'); 38 | E6 = uicontrol('units','normalized','position',[.11 .35 .1 .07],'style','edit','fontsize',13,'string',0,'backgroundcolor','w'); 39 | 40 | uicontrol('units','normalized','position',[.02 .83 .05 .07],'style','text','fontsize',13,'string','Altitude','backgroundcolor','w'); 41 | % uicontrol('units','normalized','position',[.02 .73 .05 .07],'style','text','fontsize',13,'string','Roll','backgroundcolor','w'); 42 | % uicontrol('units','normalized','position',[.02 .63 .05 .07],'style','text','fontsize',13,'string','Pitch','backgroundcolor','w'); 43 | uicontrol('units','normalized','position',[.02 .53 .05 .07],'style','text','fontsize',13,'string','Yaw','backgroundcolor','w'); 44 | uicontrol('units','normalized','position',[.02 .43 .05 .07],'style','text','fontsize',13,'string','X','backgroundcolor','w'); 45 | uicontrol('units','normalized','position',[.02 .33 .05 .07],'style','text','fontsize',13,'string','Y','backgroundcolor','w'); 46 | 47 | uicontrol('units','normalized','position',[.11 .25 .1 .07],'style','pushbutton','fontsize',13,'string','Go','callback',@Go1); 48 | 49 | % Motors speed 50 | uicontrol('units','normalized','position',[.85 .83 .05 .07],'style','text','fontsize',13,'string','Front M','backgroundcolor',[.5 .7 1]); 51 | uicontrol('units','normalized','position',[.85 .73 .05 .07],'style','text','fontsize',13,'string','Right M','backgroundcolor',[.5 .7 1]); 52 | uicontrol('units','normalized','position',[.85 .63 .05 .07],'style','text','fontsize',13,'string','Rear M','backgroundcolor',[.5 .7 1]); 53 | uicontrol('units','normalized','position',[.85 .53 .05 .07],'style','text','fontsize',13,'string','Left M','backgroundcolor',[.5 .7 1]); 54 | 55 | O1 = uicontrol('units','normalized','position',[.91 .86 .08 .05],'style','text','fontsize',13,'string','0','backgroundcolor','w'); 56 | O2 = uicontrol('units','normalized','position',[.91 .76 .08 .05],'style','text','fontsize',13,'string','0','backgroundcolor','w'); 57 | O3 = uicontrol('units','normalized','position',[.91 .66 .08 .05],'style','text','fontsize',13,'string','0','backgroundcolor','w'); 58 | O4 = uicontrol('units','normalized','position',[.91 .56 .08 .05],'style','text','fontsize',13,'string','0','backgroundcolor','w'); 59 | 60 | % disturbances 61 | % uicontrol('units','normalized','position',[.13+.77 .35 .08 .07],'style','pushbutton','fontsize',13,'string','Z','callback',@d1); 62 | % uicontrol('units','normalized','position',[.02+.77 .35 .08 .07],'style','pushbutton','fontsize',13,'string','Yaw','callback',@d2); 63 | % uicontrol('units','normalized','position',[.13+.77 .25 .08 .07],'style','pushbutton','fontsize',13,'string','Pitch','callback',@d3); 64 | % uicontrol('units','normalized','position',[.02+.77 .25 .08 .07],'style','pushbutton','fontsize',13,'string','Roll','callback',@d4); 65 | 66 | pop1 = uicontrol('units','normalized','position',[.02 .15 .19 .07],'style','popupmenu','fontsize',13,'string',{'3D view';'Camera view'},'callback',@view1,'value',1); 67 | 68 | axis([-5 5 -5 5 0 2]) 69 | view(30,30) 70 | grid on 71 | hold on 72 | %---------- Camera --------------------% 73 | 74 | 75 | camproj perspective 76 | camva(25) 77 | 78 | hlight = camlight('headlight'); 79 | 80 | lighting gouraud 81 | set(gcf,'Renderer','OpenGL') 82 | 83 | line([-1 1],[0 0],[0 0]) 84 | line([0 0],[-.5 .5],[0 0],'color','r') 85 | 86 | %---------- starting the loop ----------% 87 | while 1 88 | tic; 89 | %---------- measuring the parameters ----------% 90 | Z_meas; 91 | XY_meas; 92 | IMU_meas; 93 | 94 | %---------- Kalman filters ----------% 95 | Kalman_phi2; 96 | Kalman_theta2; 97 | Kalman_psi2; 98 | Kalman_Z2; 99 | Kalman_X2; 100 | Kalman_Y2; 101 | 102 | %---------- perform PID controllers ----------% 103 | PID_X; 104 | PID_Y; 105 | PID_Z; 106 | PID_roll; 107 | PID_pitch; 108 | PID_heading; 109 | % % 110 | % PID_X_NC; 111 | % PID_Y_NC; 112 | % PID_Z_NC; 113 | % PID_roll_NC; 114 | % PID_pitch_NC; 115 | % PID_heading_NC; 116 | 117 | %----------- limit the motors speed ----------% 118 | Motors_Speed; 119 | Forces; 120 | 121 | %---------- apply the equations of motion -----------% 122 | quadmodel; 123 | % toc 124 | % for i=1:length(xd)-50 125 | 126 | % end 127 | 128 | %---------- Calculating Motors speed and displaying it ------------% 129 | 130 | set(O1,'string',num2str(A.O1)); 131 | set(O2,'string',num2str(A.O2)); 132 | set(O3,'string',num2str(A.O3)); 133 | set(O4,'string',num2str(A.O4)); 134 | 135 | %---------- PLotting the quadrotor new position ----------% 136 | if(A.flag==3) 137 | 138 | plot_quad_3D; 139 | switch(A.Camera_View) 140 | case 0 141 | campos([A.X+2 A.Y+2 A.Z+2]) 142 | camtarget([A.X A.Y A.Z]) 143 | camroll(0); 144 | case 1 145 | campos([A.X A.Y A.Z]) 146 | camtarget([A.X+A.Z*sin(A.theta) A.Y+A.Z*sin(A.phi) 0]) 147 | camroll(A.psi*180/pi-A.psi_cam) 148 | A.psi_cam = A.psi*180/pi; 149 | end 150 | 151 | 152 | drawnow 153 | A.flag=0; 154 | toc 155 | while(toc<.03) 156 | end 157 | end 158 | A.flag = A.flag+1; 159 | 160 | A.init = 1; % stop the initilization in my code 161 | end 162 | 163 | 164 | 165 | 166 | %---------- Subfunctions -----------% 167 | 168 | function Go1(varargin) 169 | A.Z_des = str2double(get(E1,'string')); 170 | % A.phi_des = str2double(get(E2,'string'))*pi/180; 171 | % A.theta_des = str2double(get(E3,'string'))*pi/180; 172 | A.psi_des = str2double(get(E4,'string'))*pi/180; 173 | A.X_des_EF = str2double(get(E5,'string')); 174 | A.Y_des_EF = str2double(get(E6,'string')); 175 | end 176 | 177 | % disturbances sub functions 178 | % function d1(varargin) % disturbance in Z 179 | % A.Z_dis = -0.2*A.m*A.g; 180 | % end 181 | % 182 | % function d2(varargin) % disturbance in Yaw 183 | % A.psi_dis = -200*A.Izz; 184 | % end 185 | % 186 | % function d3(varargin) % disturbance in Pitch 187 | % A.theta_dis = -200*A.Iyy; 188 | % end 189 | % 190 | % function d4(varargin) % disturbance in Roll 191 | % A.phi_dis = -200*A.Ixx; 192 | % end 193 | 194 | 195 | function view1(varargin) 196 | A.var1 = get(pop1,'value'); 197 | switch A.var1 198 | case 1 199 | A.Camera_View = 0; 200 | case 2 201 | A.Camera_View = 1; 202 | end 203 | end 204 | 205 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/1.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Airmodel.m: -------------------------------------------------------------------------------- 1 | global A 2 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Forces.m: -------------------------------------------------------------------------------- 1 | function Forces 2 | global A 3 | % this function will calculates the forces and torques on the quadrotor 4 | % give the speed of each propeler. 5 | 6 | % Inputs 7 | % O1 = Front (CCW) 8 | % O2 = Right (CW) 9 | % O3 = Rear (CCW) 10 | % O4 = Left (CW) 11 | 12 | % Outputs 13 | % U1 = Throttle 14 | % U2 = Roll movement 15 | % U3 = Pitch movement 16 | % U4 = Yaw movement 17 | 18 | % Constants 19 | % l = Where l [m] is the distance between the center of the quadrotor and 20 | % the center of a propeller. 21 | % b = 22 | 23 | % A.U11_plot(A.counter) = A.U1; 24 | A.U1 = A.b*(sign(A.O1)*A.O1^2 + sign(A.O2)*A.O2^2 + sign(A.O3)*A.O3^2 + sign(A.O4)*A.O4^2); 25 | A.U1_plot(A.counter) = A.U1; 26 | 27 | % A.U21_plot(A.counter) = A.U1; 28 | A.U2 = A.b*A.l*(sign(A.O4)*A.O4^2 - sign(A.O2)*A.O2^2); 29 | A.U2_plot(A.counter) = A.U2; 30 | 31 | % A.U31_plot(A.counter) = A.U1; 32 | A.U3 = A.b*A.l*(sign(A.O3)*A.O3^2 - sign(A.O1)*A.O1^2); 33 | A.U3_plot(A.counter) = A.U3; 34 | 35 | % A.U41_plot(A.counter) = A.U1; 36 | A.U4 = A.d*(sign(A.O2)*A.O2^2 + sign(A.O4)*A.O4^2 - sign(A.O1)*A.O1^2 - sign(A.O3)*A.O3^2); 37 | A.U4_plot(A.counter) = A.U4; 38 | 39 | A.O = (-A.O1 + A.O2 - A.O3 + A.O4); 40 | A.O_plot(A.counter) = A.O; 41 | 42 | 43 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Go1.m: -------------------------------------------------------------------------------- 1 | global A -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/IMU_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter); %erros is +- 2 mrad 5 | A.theta_meas = A.theta+A.theta_error(A.counter); %erros is +- 2 mrad 6 | A.psi_meas = A.psi+A.psi_error(A.counter); %erros is +- 2 mrad 7 | 8 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/Kalman.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_X.m: -------------------------------------------------------------------------------- 1 | function Kalman_X 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (A.U1/A.m*A.theta_kalman - (A.X_dot*cos(A.psi_kalman)+A.Y_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_X2.m: -------------------------------------------------------------------------------- 1 | function Kalman_X2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((sin(A.psi_kalman)*sin(A.phi_kalman) + cos(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.X_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_Y.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.U1/A.m*A.phi_kalman - (A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | A.Y_kalman 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_Y2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((-cos(A.psi_kalman)*sin(A.phi_kalman) + sin(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.Y_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_Z.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_Z2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + (cos(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_function1.m: -------------------------------------------------------------------------------- 1 | function xhat = Kalman_function1(z,xpp,RR,QQ) 2 | persistent Pp Q R xp A H I 3 | if isempty(Pp) 4 | Pp=1; Q=QQ; R=RR; 5 | xp=xpp; A=0.5; H=1; I=eye(1); 6 | end 7 | % Compute priori 8 | xp=A*xp; 9 | Pp=A*Pp*A'+Q; 10 | % Measurement update 11 | K = Pp*H'*inv(H*Pp*H'+R); 12 | xhat = xp+K*(z-H*xp); 13 | % Update covariance 14 | Pp=(I-K*H)*Pp; 15 | xp = xhat; 16 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_phi.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.p*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_phi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(A.p + sin(A.phi_kalman)*tan(A.theta_kalman)*A.q + cos(A.phi_kalman)*tan(A.theta_kalman)*A.r)*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_psi.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.r*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_psi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(sin(A.phi_kalman)/cos(A.theta_kalman)*A.q + cos(A.phi_kalman)/cos(A.theta_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_theta.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.q*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalman_theta2.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(cos(A.phi_kalman)*A.q - sin(A.phi_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Kalmanfilter_prob1_sfun.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/Kalmanfilter_prob1_sfun.mexw64 -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/LIDAR_Plot.m: -------------------------------------------------------------------------------- 1 | 2 | global A 3 | 4 | % simLIDARinitial; 5 | % A.LIDARBeams = 4*ones(181,1) 6 | 7 | A.X22 = A.LIDARBeams.*cos(A.theta11) + A.LIDAR_X_offset; 8 | A.Y22 = A.LIDARBeams.*sin(A.theta11) + A.LIDAR_Y_offset; 9 | A.Z22 = zeros(1,181) + A.LIDAR_Z_offset; 10 | % 11 | % A.theta = 0*45*pi/180; 12 | % A.phi = 0*45*pi/180; 13 | % A.psi = 45*pi/180; 14 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X22,A.Y22,A.Z22,A.phi,A.theta,A.psi); 15 | % Z 16 | set(A.LIDAR_Patch,'xdata',A.X22'+A.X,'ydata',A.Y22'+A.Y,'zdata',A.Z22+A.Z) 17 | 18 | % axis([-4 4 -4 4 -4 4]) 19 | 20 | % grid on 21 | % view(50,50) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Map.m: -------------------------------------------------------------------------------- 1 | global A 2 | % imshow('AUS.jpg') 3 | A.aus = imread('AUS.jpg'); 4 | axis equal 5 | % [x,y,z] = sphere(50); 6 | A.t1=-250:1:249; 7 | A.c1=-250:1:249; 8 | A.x1=[]; 9 | A.y1=[]; 10 | A.z1=zeros(500,500); 11 | A.b=0; 12 | while A.b<500; 13 | A.x1=[A.x1;A.c1]; 14 | A.y1=[A.y1;A.t1]; 15 | A.b = A.b+1; 16 | end 17 | A.y1=A.y1'; 18 | A.props.AmbientStrength = 0.1; 19 | A.props.DiffuseStrength = 1; 20 | A.props.SpecularColorReflectance = .5; 21 | A.props.SpecularExponent = 20; 22 | A.props.SpecularStrength = 1; 23 | A.props.FaceColor= 'texture'; 24 | A.props.EdgeColor = 'none'; 25 | A.props.FaceLighting = 'phong'; 26 | A.props.Cdata = A.aus; 27 | surface(A.x1,A.y1,A.z1,A.props); -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Motors_Speed.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if abs(BB1)>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if abs(BB2)>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if abs(BB3)>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if abs(BB4)>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | A.O1 = sign(BB1)*sqrt(abs(BB1)); % Front M 26 | A.O2 = sign(BB2)*sqrt(abs(BB2)); % Right M 27 | A.O3 = sign(BB3)*sqrt(abs(BB3)); % Rear M 28 | A.O4 = sign(BB4)*sqrt(abs(BB4)); % Left M 29 | 30 | A.O1_plot(A.counter) = A.O1; 31 | A.O2_plot(A.counter) = A.O2; 32 | A.O3_plot(A.counter) = A.O3; 33 | A.O4_plot(A.counter) = A.O4; 34 | 35 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Motors_Speed_2.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed_2 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if BB1>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if BB2>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if BB3>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if BB4>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | if BB1 pi/4) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/4; 13 | end 14 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_X_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.X_des = A.X_des_EF*cos(A.psi_meas)+A.Y_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | 5 | A.X_BF = A.X_meas*cos(A.psi_meas)+A.Y_meas*sin(A.psi_meas); % calculating X in BF 6 | A.X_dot_BF = A.X_dot*cos(A.psi_meas)+A.Y_dot*sin(A.psi_meas); % calculating X_dot in BF 7 | 8 | % PD controller for X_position 9 | A.theta_des = A.X_KP*(A.X_des - A.X_BF) + A.X_KD*A.X_dot; 10 | 11 | if(abs(A.theta_des) > pi/18) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/18; 13 | end 14 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_Y.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_kalman)-A.X_des_EF*sin(A.psi_kalman); % calculating the desired X in BF 4 | A.Y_BF = A.Y_kalman*cos(A.psi_kalman)-A.X_kalman*sin(A.psi_kalman); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/4) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/4; 12 | end 13 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_Y_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_meas)-A.X_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | A.Y_BF = A.Y_meas*cos(A.psi_meas)-A.X_meas*sin(A.psi_meas); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_meas)-A.X_dot*sin(A.psi_meas); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/18) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/18; 12 | end 13 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_Z.m: -------------------------------------------------------------------------------- 1 | function PID_Z 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_kalman; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_kalman)/cos(A.theta_kalman); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_Z_NC.m: -------------------------------------------------------------------------------- 1 | function PID_Z_NC 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_meas; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_meas)/cos(A.theta_meas); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_heading.m: -------------------------------------------------------------------------------- 1 | function PID_heading 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_kalman ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_heading_NC.m: -------------------------------------------------------------------------------- 1 | function PID_heading_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_meas ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_pitch.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_kalman; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_pitch_NC.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch_NC 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_meas; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_roll.m: -------------------------------------------------------------------------------- 1 | function PID_roll 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_kalman; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/PID_roll_NC.m: -------------------------------------------------------------------------------- 1 | function PID_roll_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_meas; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Path_manager.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | % hold on 4 | % if(A.init==0) 5 | % plot3(A.X_path,A.Y_path,A.Z_path,'+r','linewidth',3) 6 | % end 7 | 8 | if(A.path_counter <= length(A.X_path)) 9 | A.X_des_EF = A.X_path(A.path_counter); 10 | A.Y_des_EF = A.Y_path(A.path_counter); 11 | A.Z_des = A.Z_path(A.path_counter); 12 | else 13 | plot_XY; 14 | A.flagggg=1; 15 | end 16 | % A.X_des_EF 17 | % A.Y_des_EF 18 | % A.path_counter 19 | % A.X_kalman 20 | % abs(A.X_kalman-A.X_des)<.02 21 | if((abs(A.X_kalman-A.X_des_EF)<.04)&&(abs(A.Y_kalman-A.Y_des_EF)<.04)&&(abs(A.Z_kalman-A.Z_des)<.08)) 22 | A.path_counter = A.path_counter+1; 23 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Quadrotor.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/Quadrotor.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Quadrotor_sfun.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/Quadrotor_sfun.mexw64 -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/READ ME.txt: -------------------------------------------------------------------------------- 1 | Run script2run.m 2 | 3 | Enjoy :) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/SimInitial.m: -------------------------------------------------------------------------------- 1 | function SimInitial 2 | global A 3 | % 4 | % A.X1wall = 0; % X first point in wall NF were X is on the right direction and Y is in front !! 5 | % A.Y1wall = 1; % Y first point in wall 6 | % A.X2wall = .5; % X second point in wall X2 has to be > X1 7 | % A.Y2wall = 1; % Y second point in wall 8 | % 9 | % A.Z1wall = 0; % Z lower edge 10 | % A.Z2wall = 1; % Z higher edge 11 | 12 | % D1 B1 C1 are three points on the wall that form a plane [X,Y,Z] 13 | % the ordering of the points will be like this 14 | % B1 = [X1,Y1,Z1] 15 | % C1 = [X2,Y2,Z1] 16 | % D1 = [(X1+X2)/2,(Y1+Y2)/2,Z2] 17 | 18 | % obstacle num 1 19 | A.B1(1,:) = [-1,1,0]; 20 | A.C1(1,:) = [1,1,0]; 21 | A.D1(1,:) = [.5,1,1]; 22 | 23 | %obstacle num 2 24 | A.B1(2,:) = [0,2,0]; 25 | A.C1(2,:) = [2,2,0]; 26 | A.D1(2,:) = [1,2,2]; 27 | 28 | %obstacle num 3 29 | A.B1(3,:) = [-2,1,0]; 30 | A.C1(3,:) = [-1,3,0]; 31 | A.D1(3,:) = [-1.5,2,1.5]; 32 | 33 | A.num_obstacles = size(A.D1); 34 | 35 | for i=1:A.num_obstacles(1) 36 | 37 | patch([A.B1(i,1) A.C1(i,1) A.C1(i,1) A.B1(i,1)],[A.B1(i,2) A.C1(i,2) A.C1(i,2) A.B1(i,2)],[A.B1(i,3) A.B1(i,3) A.D1(i,3) A.D1(i,3)],[0 .4 1]) 38 | 39 | DB = A.B1(i,:)-A.D1(i,:); 40 | DC = A.C1(i,:)-A.D1(i,:); 41 | 42 | A.M1(i) = DB(2)*DC(3) - DB(3)*DC(2); 43 | A.M2(i) = DB(1)*DC(3) - DB(3)*DC(1); 44 | A.M3(i) = DB(1)*DC(2) - DB(2)*DC(1); 45 | end 46 | 47 | A.X1 = 0; % quadrotors position X 48 | A.Y1 = 0; % quadrotors position Y 49 | A.Z1 = .5; % quadrotors position Z 50 | 51 | A.theta11 = [0:pi/180:pi]; % used in ploting the LIDAR beams 52 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/SimInitial2.m: -------------------------------------------------------------------------------- 1 | function SimInitial2 2 | global A 3 | % 4 | % A.X1wall = 0; % X first point in wall NF were X is on the right direction and Y is in front !! 5 | % A.Y1wall = 1; % Y first point in wall 6 | % A.X2wall = .5; % X second point in wall X2 has to be > X1 7 | % A.Y2wall = 1; % Y second point in wall 8 | % 9 | % A.Z1wall = 0; % Z lower edge 10 | % A.Z2wall = 1; % Z higher edge 11 | 12 | % D1 B1 C1 are three points on the wall that form a plane [X,Y,Z] 13 | % the ordering of the points will be like this 14 | % B1 = [X1,Y1,Z1] 15 | % C1 = [X2,Y2,Z1] 16 | % D1 = [(X1+X2)/2,(Y1+Y2)/2,Z2] 17 | 18 | % samll room obstacle 19 | Xi = [-1.5 -1.5 .5 -1.5 -1.5 -1.5 3 1.5 1.5]; 20 | Xf = [1.5 -.5 1.5 1.5 -1.5 3 3 3 1.5]; 21 | Yi = [1 1 1 1 1 5 2 2 1]; 22 | Yf = [1 1 1 1 5 5 5 2 2]; 23 | Zi = [0 .5 .5 1 0 0 0 0 0]; 24 | Zf = [.5 1 1 1.5 1.5 1.5 1.5 1.5 1.5]; 25 | 26 | 27 | % random wall 28 | % Xi = [-1.5 -3 -1]; 29 | % Xf = [1 -2.5 1.5]; 30 | % Yi = [1 -.5 2.2]; 31 | % Yf = [1 2 2.2]; 32 | % Zi = [0 0 0]; 33 | % Zf = [1.5 2.5 3]; 34 | 35 | for i=1:length(Xi) 36 | [A.B1(i,:),A.C1(i,:),A.D1(i,:)] = obstacle([Xi(i) Xf(i)],[Yi(i) Yf(i)],[Zi(i) Zf(i)]); 37 | % [A.B1(2,:),A.C1(2,:),A.D1(2,:)] = obstacle([-1.5 -.5],[1 1],[.5 1]); 38 | end 39 | 40 | % A.B1(1,:) = [-1,1,0]; 41 | % A.C1(1,:) = [1,1,0]; 42 | % A.D1(1,:) = [.5,1,1]; 43 | 44 | %obstacle num 2 45 | % A.B1(2,:) = [0,2,0]; 46 | % A.C1(2,:) = [2,2,0]; 47 | % A.D1(2,:) = [1,2,2]; 48 | 49 | %obstacle num 3 50 | % A.B1(3,:) = [-2,1,0]; 51 | % A.C1(3,:) = [-1,3,0]; 52 | % A.D1(3,:) = [-1.5,2,1.5]; 53 | 54 | A.num_obstacles = size(A.D1); 55 | 56 | for i=1:A.num_obstacles(1) 57 | 58 | patch([A.B1(i,1) A.C1(i,1) A.C1(i,1) A.B1(i,1)],[A.B1(i,2) A.C1(i,2) A.C1(i,2) A.B1(i,2)],[A.B1(i,3) A.B1(i,3) A.D1(i,3) A.D1(i,3)],[0 .4 1]) 59 | 60 | DB = A.B1(i,:)-A.D1(i,:); 61 | DC = A.C1(i,:)-A.D1(i,:); 62 | 63 | A.M1(i) = DB(2)*DC(3) - DB(3)*DC(2); 64 | A.M2(i) = DB(1)*DC(3) - DB(3)*DC(1); 65 | A.M3(i) = DB(1)*DC(2) - DB(2)*DC(1); 66 | end 67 | 68 | A.X1 = 0; % quadrotors position X 69 | A.Y1 = 0; % quadrotors position Y 70 | A.Z1 = .5; % quadrotors position Z 71 | 72 | A.theta11 = [0:pi/180:pi]; % used in ploting the LIDAR beams 73 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Sim_LIDAR.m: -------------------------------------------------------------------------------- 1 | function Sim_LIDAR 2 | 3 | global A 4 | % 5 | A.X1 = A.X + A.LIDAR_X_offset; 6 | A.Y1 = A.Y + A.LIDAR_Y_offset; 7 | A.Z1 = A.Z + A.LIDAR_Z_offset; 8 | 9 | % A.X1 = A.X; 10 | % A.Y1 = A.Y; 11 | % A.Z1 = A.Z; 12 | 13 | tic 14 | 15 | [A.X2,A.Y2,A.Z2]=rotateXYZ(A.X2_init,A.Y2_init,A.Z2_init,A.phi,A.theta,A.psi); 16 | A.Z2 = A.Z2+A.Z1; 17 | A.X2 = A.X2+A.X1; 18 | A.Y2 = A.Y2+A.Y1; 19 | 20 | Beam_distances = 4*ones(1,181); 21 | Beam_distances_ground = 4*ones(1,181); 22 | A.LIDARBeams = 4*ones(1,181); 23 | %--------------- Obstacles --------------------% 24 | for i=1:A.num_obstacles(1) 25 | 26 | m = -1*(A.M1(i)*(A.X1*ones(1,181)-A.D1(i,1)) - A.M2(i)*(A.Y1*ones(1,181)-A.D1(i,2)) + A.M3(i)*(A.Z1*ones(1,181)-A.D1(i,3)))./(A.M1(i)*(A.X2-A.X1) - A.M2(i)*(A.Y2-A.Y1) + A.M3(i)*(A.Z2-A.Z1)); 27 | 28 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 29 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 30 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 31 | 32 | % check if the intersection is in the same direction of them beam or not 33 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 34 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 35 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 36 | 37 | % intersection with the obstacle 38 | BI5 = (X_suggested>=A.B1(i,1)-eps*10); 39 | BI6 = (X_suggested<=A.C1(i,1)+eps*10); 40 | BI7 = (Y_suggested>=A.B1(i,2)-eps*10); 41 | BI8 = (Y_suggested<=A.C1(i,2)+eps*10); 42 | BI9 = (Z_suggested>=A.B1(i,3)-eps*10); 43 | BI10 = (Z_suggested<=A.D1(i,3)+eps*10); 44 | 45 | BI = ((BI1&BI2&BI3)&BI5&BI6&BI7&BI8&BI9&BI10); 46 | Beam_distances(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 47 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances); 48 | end 49 | %------------------ Ground ------------------------% 50 | m = -1*A.Z1*ones(1,181)./((A.Z2-A.Z1)); 51 | 52 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 53 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 54 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 55 | 56 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 57 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 58 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 59 | BI = (BI1&BI2&BI3); 60 | 61 | Beam_distances_ground(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 62 | 63 | % A.LIDARBeams = min(A.LIDARBeams,Beam_distances); 64 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances_ground); 65 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Sim_LIDAR_2.m: -------------------------------------------------------------------------------- 1 | function Sim_LIDAR_2 2 | 3 | global A 4 | 5 | A.X1 = A.X; 6 | A.Y1 = A.Y; 7 | A.Z1 = A.Z; 8 | 9 | tic 10 | 11 | [A.X2,A.Y2,A.Z2]=rotateXYZ(A.X2_init,A.Y2_init,A.Z2_init,A.theta,A.phi,A.psi); 12 | A.Z2 = A.Z2+A.Z1; 13 | A.X2 = A.X2+A.X1; 14 | A.Y2 = A.Y2+A.Y1; 15 | 16 | Beam_distances = 4*ones(1,181); 17 | Beam_distances_ground = 4*ones(1,181); 18 | A.LIDARBeams = 4*ones(1,181); 19 | %--------------- Obstacles -------------------- 20 | m = -1*(A.M1*(A.X1*ones(1,181)-A.D1(1)) - A.M2*(A.Y1*ones(1,181)-A.D1(2)) + A.M3*(A.Z1*ones(1,181)-A.D1(3)))./(A.M1*(A.X2-A.X1) - A.M2*(A.Y2-A.Y1) + A.M3*(A.Z2-A.Z1)); 21 | 22 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 23 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 24 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 25 | 26 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 27 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 28 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 29 | 30 | BI5 = (X_suggested>=-1); 31 | BI6 = (X_suggested<=1); 32 | BI7 = (Y_suggested>=0); 33 | BI8 = (Y_suggested<=2); 34 | BI9 = (Z_suggested>=0); 35 | BI10 = (Z_suggested<=(Y_suggested/2)); 36 | 37 | BI = ((BI1&BI2&BI3)&BI5&BI6&BI7&BI8&BI9&BI10); 38 | Beam_distances(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 39 | 40 | 41 | %------------------ Ground ------------------------% 42 | m = -1*A.Z1*ones(1,181)./((A.Z2-A.Z1)); 43 | 44 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 45 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 46 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 47 | 48 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 49 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 50 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 51 | BI = (BI1&BI2&BI3); 52 | 53 | Beam_distances_ground(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 54 | 55 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances); 56 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances_ground); 57 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/XY_meas.m: -------------------------------------------------------------------------------- 1 | function XY_meas 2 | global A 3 | 4 | A.X_meas = A.X+A.X_error(A.counter); %erros is +- 1 cm 5 | A.Y_meas = A.Y+A.Y_error(A.counter); %erros is +- 1 cm 6 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Y_Backstepping.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | if(A.U1~=0) 4 | A.U2 = .1*(-5*(A.Y - A.Y_des) -10*A.Y_dot - 9*A.U1*A.S_psi*A.C_phi ... 5 | -4*A.U1*A.psi_dot*A.C_psi*A.C_phi + A.U1*A.psi_dot^2*A.S_psi*A.C_phi ... 6 | +2*A.U1*A.psi_dot*A.S_psi*A.S_phi + A.U1*A.psi_dot*A.phi_dot*A.C_psi*A.S_phi ... 7 | -A.U1*A.phi_dot*A.psi_dot*A.C_psi*A.S_phi - A.U1*A.phi_dot^2*A.S_psi*A.C_phi)/(A.U1*A.C_psi*A.C_phi); 8 | else 9 | A.U2 = 0; 10 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/Z_meas.m: -------------------------------------------------------------------------------- 1 | function Z_meas 2 | global A 3 | 4 | A.Z_meas = A.Z+A.Z_error(A.counter); %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/copter_model.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | x1=-.27; 4 | x2=.27; 5 | y1=-.01; 6 | y2=.01; 7 | z1=-.01; 8 | z2=.01; 9 | 10 | acon=[x1 y1 z1; 11 | x2 y1 z1; 12 | x2 y2 z1; 13 | x1 y2 z1; 14 | x1 y1 z2; 15 | x2 y1 z2; 16 | x2 y2 z2; 17 | x1 y2 z2]; 18 | 19 | bcon=[1 2 6 5; 20 | 2 3 7 6; 21 | 3 4 8 7; 22 | 4 1 5 8; 23 | 1 2 3 4; 24 | 5 6 7 8]; 25 | 26 | 27 | body1=patch('faces',bcon,... 28 | 'vertices',acon,... 29 | 'facecolor',[.8 .8 .8],... 30 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 31 | 32 | y1=-.27; 33 | y2=.27; 34 | x1=-.01; 35 | x2=.01; 36 | z1=-.01; 37 | z2=.01; 38 | 39 | acon=[x1 y1 z1; 40 | x2 y1 z1; 41 | x2 y2 z1; 42 | x1 y2 z1; 43 | x1 y1 z2; 44 | x2 y1 z2; 45 | x2 y2 z2; 46 | x1 y2 z2]; 47 | 48 | body2=patch('faces',bcon,... 49 | 'vertices',acon,... 50 | 'facecolor',[.8 .8 .8],... 51 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 52 | 53 | t=0:pi/10:2*pi; 54 | X=.13*cos(t); 55 | Y=.13*sin(t); 56 | Z=zeros(size(t))+.015; 57 | C=zeros(size(t)); 58 | A.C = C; 59 | 60 | RotorF = patch(X+.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 61 | RotorB = patch(X-.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 62 | RotorL = patch(X,Y+.27,Z,C,'facealpha',.3,'facecolor','k'); 63 | RotorR = patch(X,Y-.27,Z,C,'facealpha',.3,'facecolor','k'); 64 | 65 | A.Body1X = get(body1,'xdata'); 66 | A.Body1Y = get(body1,'ydata'); 67 | A.Body1Z = get(body1,'zdata'); 68 | 69 | A.Body2X = get(body2,'xdata'); 70 | A.Body2Y = get(body2,'ydata'); 71 | A.Body2Z = get(body2,'zdata'); 72 | 73 | A.RotorFX = get(RotorF,'xdata'); 74 | A.RotorFY = get(RotorF,'ydata'); 75 | A.RotorFZ = get(RotorF,'zdata'); 76 | 77 | A.RotorBX = get(RotorB,'xdata'); 78 | A.RotorBY = get(RotorB,'ydata'); 79 | A.RotorBZ = get(RotorB,'zdata'); 80 | 81 | A.RotorRX = get(RotorR,'xdata'); 82 | A.RotorRY = get(RotorR,'ydata'); 83 | A.RotorRZ = get(RotorR,'zdata'); 84 | 85 | A.RotorLX = get(RotorL,'xdata'); 86 | A.RotorLY = get(RotorL,'ydata'); 87 | A.RotorLZ = get(RotorL,'zdata'); 88 | 89 | % xlabel('x') 90 | % grid on 91 | % axis([-1 1 -1 1 -1 1]); 92 | axis equal 93 | 94 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/copter_model_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | load Quadrotor 4 | 5 | A.Body1 = patch('xdata',A.Body1X,'ydata',A.Body1Y,'zdata',A.Body1Z,'facealpha',.3,'facecolor','b'); 6 | A.Body2 = patch('xdata',A.Body2X,'ydata',A.Body2Y,'zdata',A.Body2Z,'facealpha',.3,'facecolor','b'); 7 | A.RotorF = patch('xdata',A.RotorFX,'ydata',A.RotorFY,'zdata',A.RotorFZ,'facealpha',.3,'facecolor','k'); 8 | A.RotorB = patch('xdata',A.RotorBX,'ydata',A.RotorBY,'zdata',A.RotorBZ,'facealpha',.3,'facecolor','k'); 9 | A.RotorR = patch('xdata',A.RotorRX,'ydata',A.RotorRY,'zdata',A.RotorRZ,'facealpha',.3,'facecolor','k'); 10 | A.RotorL = patch('xdata',A.RotorLX,'ydata',A.RotorLY,'zdata',A.RotorLZ,'facealpha',.3,'facecolor','k'); 11 | 12 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/example.m: -------------------------------------------------------------------------------- 1 | load wind 2 | wind_speed = sqrt(u.^2 + v.^2 + w.^2); 3 | 4 | % hpatch = patch(isosurface(x,y,z,wind_speed,35)); 5 | % isonormals(x,y,z,wind_speed,hpatch) 6 | % set(hpatch,'FaceColor','red','EdgeColor','none'); 7 | % 8 | % [f vt] = reducepatch(isosurface(x,y,z,wind_speed,45),0.05); 9 | % daspect([1,1,1]); 10 | % hcone = coneplot(x,y,z,u,v,w,vt(:,1),vt(:,2),vt(:,3),2); 11 | % set(hcone,'FaceColor','blue','EdgeColor','none'); 12 | 13 | camproj perspective 14 | camva(25) 15 | 16 | hlight = camlight('headlight'); 17 | % set(hpatch,'AmbientStrength',.1,... 18 | % 'SpecularStrength',1,... 19 | % 'DiffuseStrength',1); 20 | % set(hcone,'SpecularStrength',1); 21 | % set(gcf,'Color','k') 22 | 23 | lighting gouraud 24 | set(gcf,'Renderer','OpenGL') 25 | 26 | hsline = streamline(x,y,z,u,v,w,80,30,11); 27 | xd = get(hsline,'XData'); 28 | yd = get(hsline,'YData'); 29 | zd = get(hsline,'ZData'); 30 | delete(hsline) 31 | tic 32 | for i=1:length(xd)-50 33 | campos([xd(i),yd(i),zd(i)]) 34 | camtarget([xd(i+5)+min(xd)/100,yd(i),zd(i)]) 35 | camlight(hlight,'headlight') 36 | drawnow 37 | toc 38 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/obstacle.m: -------------------------------------------------------------------------------- 1 | function [B,C,D] = obstacle(X,Y,Z) 2 | % B1 = [X1,Y1,Z1] 3 | % C1 = [X2,Y2,Z1] 4 | % D1 = [(X1+X2)/2,(Y1+Y2)/2,Z2] 5 | 6 | B = [X(1),Y(1),Z(1)]; 7 | C = [X(2),Y(2),Z(1)]; 8 | D = [(X(1)+X(2))/2,(Y(1)+Y(2))/2,Z(2)]; 9 | 10 | 11 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/obstacles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/obstacles.png -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/phi_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter) %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_X.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter)+A.X_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.X_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.X_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.X_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_XY.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | 7 | plot3(A.X_plot(1:A.counter-1),A.Y_plot(1:A.counter-1),A.Z_plot(1:A.counter-1)) 8 | plot3(A.X_path,A.Y_path,A.Z_path,'+r','linewidth',3) 9 | view(70,50) 10 | grid on 11 | axis equal 12 | axis([-4 2 -1 3 0 2]) 13 | % plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter),'r','linewidth',1) 14 | % plot(A.t_plot(1:A.counter),A.Y_ref_plot(1:A.counter),'b') 15 | % plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 16 | 17 | for i=1:A.num_obstacles(1) 18 | patch([A.B1(i,1) A.C1(i,1) A.C1(i,1) A.B1(i,1)],[A.B1(i,2) A.C1(i,2) A.C1(i,2) A.B1(i,2)],[A.B1(i,3) A.B1(i,3) A.D1(i,3) A.D1(i,3)],[0 .4 1]) 19 | end 20 | 21 | legend('Quadrotor path','Desired path') 22 | 23 | xlabel('X axis (m)') 24 | ylabel('Y axis (m)') 25 | zlabel('Z axis (m)') 26 | 27 | title('path traveled') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_Y.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter)+A.Y_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.Y_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.Y_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_Z.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | hold on 6 | plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter)+A.Z_error(1:A.counter),'y') 7 | plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter),'r','linewidth',1) 8 | plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 9 | plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 10 | plot(A.t_plot(1:A.counter),A.Z_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | legend('measured response','actual response','set value','disturbances','kalman filter') 12 | 13 | xlabel('time (s)') 14 | ylabel('altitude (m)') 15 | title('altitude vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_forces.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | plot(A.t_plot(1:A.counter-1),A.U1_plot(1:A.counter-1),'r') 6 | plot(A.t_plot(1:A.counter-1),A.U2_plot(1:A.counter-1),'g') 7 | plot(A.t_plot(1:A.counter-1),A.U3_plot(1:A.counter-1),'b') 8 | plot(A.t_plot(1:A.counter-1),A.U4_plot(1:A.counter-1),'k') 9 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 11 | legend('Throttle','Roll','Pitch','Yaw') 12 | xlabel('time (s)') 13 | ylabel('value') 14 | title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_motors.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.O1_plot(1:A.counter),'r') 6 | plot(A.t_plot(1:A.counter),A.O2_plot(1:A.counter),'g') 7 | plot(A.t_plot(1:A.counter),A.O3_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.O4_plot(1:A.counter),'k') 9 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 11 | legend('front','right','rear','left') 12 | xlabel('time (s)') 13 | ylabel('value') 14 | title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_phi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter)+A.phi_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.phi_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.phi_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.phi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('phi (rad)') 13 | title('phi vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_psi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter)+A.psi_error(1:A.counter),'y') 6 | plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.psi_ref_plot(1:A.counter),'b') 8 | plot(A.t_plot(1:A.counter),A.psi_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.psi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | legend('measured response','actual response','set value','disturbances','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('psi (rad)') 13 | title('psi vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_quad.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X2,A.Y2,A.Z2,A.phi,A.theta,A.psi); 4 | set(A.L2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.FPX,A.FPY,A.FPZ,A.phi,A.theta,A.psi); 7 | set(A.L3,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.RPX,A.RPY,A.RPZ,A.phi,A.theta,A.psi); 10 | set(A.L4,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.rPX,A.rPY,A.rPZ,A.phi,A.theta,A.psi); 13 | set(A.L5,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.LPX,A.LPY,A.LPZ,A.phi,A.theta,A.psi); 16 | set(A.L6,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X1,A.Y1,A.Z1,A.phi,A.theta,A.psi); 19 | set(A.L1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 20 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_quad_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body1X,A.Body1Y,A.Body1Z,A.phi,A.theta,A.psi); 4 | set(A.Body1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body2X,A.Body2Y,A.Body2Z,A.phi,A.theta,A.psi); 7 | set(A.Body2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorFX,A.RotorFY,A.RotorFZ,A.phi,A.theta,A.psi); 10 | set(A.RotorF,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorBX,A.RotorBY,A.RotorBZ,A.phi,A.theta,A.psi); 13 | set(A.RotorB,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorLX,A.RotorLY,A.RotorLZ,A.phi,A.theta,A.psi); 16 | set(A.RotorL,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorRX,A.RotorRY,A.RotorRZ,A.phi,A.theta,A.psi); 19 | set(A.RotorR,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/plot_theta.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | hold on 5 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter)+A.theta_error(1:A.counter),'color',[.8 .8 .8]) 6 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter),'r') 7 | plot(A.t_plot(1:A.counter),A.theta_ref_plot(1:A.counter),'b') 8 | % plot(A.t_plot(1:A.counter),A.theta_dis_plot(1:A.counter),'g') 9 | plot(A.t_plot(1:A.counter),A.theta_kalman_plot(1:A.counter),'color',[0 1 0]) 10 | legend('measured response','actual response','set value','kalman filter') 11 | xlabel('time (s)') 12 | ylabel('theta (rad)') 13 | title('theta vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/quadmodel.m: -------------------------------------------------------------------------------- 1 | function quadmodel 2 | global A 3 | % this function simulate the dynamics of the quadrotor, it will take the 4 | % Euler angels and forces as inputs and give the linear accelerations WRT 5 | % E-Frame and rotational accelerations WRT B-Frame. 6 | 7 | A.X_ddot = (sin(A.psi)*sin(A.phi) + cos(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 8 | A.Y_ddot = (-cos(A.psi)*sin(A.phi) + sin(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 9 | A.Z_ddot = -A.g + (cos(A.theta)*cos(A.phi))*A.U1/A.m; 10 | 11 | A.p_dot = ((A.Iyy - A.Izz)*A.q*A.r -A.Jtp*A.q*A.O + A.U2)/A.Ixx; 12 | A.q_dot = ((A.Izz - A.Ixx)*A.p*A.r +A.Jtp*A.p*A.O + A.U3)/A.Iyy; 13 | A.r_dot = ((A.Ixx - A.Iyy)*A.p*A.q + A.U4)/A.Izz; 14 | 15 | A.phi_dot = A.p + sin(A.phi)*tan(A.theta)*A.q + cos(A.phi)*tan(A.theta)*A.r; 16 | A.theta_dot = cos(A.phi)*A.q - sin(A.phi)*A.r; 17 | A.psi_dot = sin(A.phi)/cos(A.theta)*A.q + cos(A.phi)/cos(A.theta)*A.r; 18 | 19 | % Air friction model 20 | A.X_ddot = A.X_ddot - A.X_dot*1.2; 21 | A.Y_ddot = A.Y_ddot - A.Y_dot*1.2; 22 | 23 | % Disturbance model 24 | A.Z_ddot = A.Z_ddot + A.Z_dis/A.m; 25 | 26 | A.psi_dot = A.psi_dot + A.psi_dis/A.Izz*A.Ts; 27 | 28 | A.theta_dot = A.theta_dot + A.theta_dis/A.Iyy*A.Ts; 29 | 30 | A.phi_dot = A.phi_dot + A.phi_dis/A.Ixx*A.Ts; 31 | 32 | 33 | % Calculating the Z velocity & position 34 | A.Z_dot = A.Z_ddot*A.Ts + A.Z_dot; 35 | A.Z = A.Z_dot*A.Ts + A.Z; 36 | 37 | % Calculating the X velocity & position 38 | A.X_dot = A.X_ddot*A.Ts + A.X_dot; 39 | A.X = A.X_dot*A.Ts + A.X; 40 | 41 | % Calculating the Y velocity & position 42 | A.Y_dot = A.Y_ddot*A.Ts + A.Y_dot; 43 | A.Y = A.Y_dot*A.Ts + A.Y; 44 | 45 | % Calculating p,q,r 46 | A.p = A.p_dot*A.Ts+A.p; 47 | A.q = A.q_dot*A.Ts+A.q; 48 | A.r = A.r_dot*A.Ts+A.r; 49 | 50 | % Calculating phi,theta,psi 51 | A.phi = A.phi_dot*A.Ts+A.phi; 52 | A.theta = A.theta_dot*A.Ts+A.theta; 53 | A.psi = A.psi_dot*A.Ts+A.psi; 54 | 55 | % Plotting variables 56 | A.Z_plot(A.counter) = A.Z; 57 | A.Z_ref_plot(A.counter) = A.Z_des; 58 | A.Z_dis_plot(A.counter) = A.Z_dis; 59 | 60 | A.X_plot(A.counter) = A.X; 61 | A.X_ref_plot(A.counter) = A.X_des; 62 | A.X_dis_plot(A.counter) = A.X_dis; 63 | 64 | A.Y_plot(A.counter) = A.Y; 65 | A.Y_ref_plot(A.counter) = A.Y_des; 66 | A.Y_dis_plot(A.counter) = A.Y_dis; 67 | 68 | A.phi_plot(A.counter) = A.phi; 69 | A.phi_ref_plot(A.counter) = A.phi_des; 70 | A.phi_dis_plot(A.counter) = A.phi_dis; 71 | 72 | A.theta_plot(A.counter) = A.theta; 73 | A.theta_ref_plot(A.counter) = A.theta_des; 74 | A.theta_dis_plot(A.counter) = A.theta_dis; 75 | 76 | A.psi_plot(A.counter) = A.psi; 77 | A.psi_ref_plot(A.counter) = A.psi_des; 78 | A.psi_dis_plot(A.counter) = A.psi_dis; 79 | 80 | A.counter = A.counter + 1; 81 | 82 | % Disturbance removal 83 | if(A.Z_dis ~= 0) 84 | A.Z_dis = 0; 85 | end 86 | 87 | if(A.psi_dis ~= 0) 88 | A.psi_dis = 0; 89 | end 90 | 91 | if(A.theta_dis ~= 0) 92 | A.theta_dis = 0; 93 | end 94 | 95 | if(A.phi_dis ~= 0) 96 | A.phi_dis = 0; 97 | end 98 | 99 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/rotateXYZ.m: -------------------------------------------------------------------------------- 1 | function [X,Y,Z]=rotateXYZ(X,Y,Z,phi,theta,psi) 2 | % define rotation matrix 3 | R_roll = [... 4 | 1, 0, 0;... 5 | 0, cos(phi), -sin(phi);... 6 | 0, sin(phi), cos(phi)]; 7 | R_pitch = [... 8 | cos(theta), 0, sin(theta);... 9 | 0, 1, 0;... 10 | -sin(theta), 0, cos(theta)]; 11 | R_yaw = [... 12 | cos(psi), -sin(psi), 0;... 13 | sin(psi), cos(psi), 0;... 14 | 0, 0, 1]; 15 | R = R_roll'*R_pitch'*R_yaw'; 16 | 17 | % rotate vertices 18 | for i=1:length(X) 19 | pts = [X(i), Y(i), Z(i)]*R; 20 | 21 | X(i) = pts(:,1); 22 | Y(i) = pts(:,2); 23 | Z(i) = pts(:,3); 24 | end 25 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/script2run2.m: -------------------------------------------------------------------------------- 1 | 2 | clear all 3 | clc 4 | global A 5 | All_Variables 6 | SimInitial 7 | simLIDARinitial 8 | view(30,30) 9 | grid on 10 | A.Z = .5; 11 | 12 | % for i=1:360 13 | % A.psi = A.psi +1*pi/180; 14 | % Sim_LIDAR 15 | % LIDAR_Plot 16 | % drawnow 17 | % toc 18 | % end 19 | 20 | for i=1:360 21 | A.phi = A.phi +1*pi/180; 22 | Sim_LIDAR 23 | LIDAR_Plot 24 | drawnow 25 | toc 26 | end 27 | 28 | for i=1:360 29 | A.theta = A.theta +1*pi/180; 30 | Sim_LIDAR 31 | LIDAR_Plot 32 | drawnow 33 | toc 34 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/simLIDARinitial.m: -------------------------------------------------------------------------------- 1 | function simLIDARinitial 2 | global A 3 | 4 | % A.LIDARResolution=1; 5 | % A.LIDARMaxRange=4; 6 | % A.LIDARNumberOfBeams=1+180/A.LIDARResolution; 7 | % A.LIDARBeamAngles = (0:A.LIDARResolution*pi/180:pi)'; 8 | % A.LIDARBeamSlopes = tan(A.LIDARBeamAngles); 9 | 10 | A.LIDAR_X_offset = 0; 11 | A.LIDAR_Y_offset = 0; 12 | A.LIDAR_Z_offset = 0; 13 | 14 | 15 | A.LIDAR_X = [A.X1 cos(A.theta11)] + A.LIDAR_X_offset; 16 | A.LIDAR_Y = [A.Y1 sin(A.theta11)] + A.LIDAR_Y_offset; 17 | A.LIDAR_Z = [A.Z1 zeros(1,181)] + A.LIDAR_Z_offset; 18 | A.Cdata = ones(1,182); 19 | 20 | A.LIDAR_Patch = patch(A.LIDAR_X,A.LIDAR_Y,A.LIDAR_Z,A.Cdata,'facecolor','r','FaceAlpha',.3); 21 | 22 | 23 | A.LIDARBeams = 4*ones(1,181); 24 | 25 | A.t=0:180; 26 | 27 | A.X2_init = 4*cosd(A.t); 28 | A.Y2_init = 4*sind(A.t); 29 | A.Z2_init = zeros(1,181); -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/binfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/binfo.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/chart1_ZfxxQE31IEuUdTwkikIDi.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/chart1_ZfxxQE31IEuUdTwkikIDi.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/chart2_9mJDJgrA6frDphDvY5VxDD.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/chart2_9mJDJgrA6frDphDvY5VxDD.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/chart3_01Yt7r9mvv6kgKbp23oyGC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/info/chart3_01Yt7r9mvv6kgKbp23oyGC.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.bat: -------------------------------------------------------------------------------- 1 | call "mexopts.bat" 2 | nmake -f Quadrotor_sfun.mak 3 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.exp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.exp -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.h: -------------------------------------------------------------------------------- 1 | #ifndef __Quadrotor_sfun_h__ 2 | #define __Quadrotor_sfun_h__ 3 | 4 | /* Include files */ 5 | #define S_FUNCTION_NAME sf_sfun 6 | #include "sfc_sf.h" 7 | #include "sfc_mex.h" 8 | #include "rtwtypes.h" 9 | #include "sfcdebug.h" 10 | #define rtInf (mxGetInf()) 11 | #define rtMinusInf (-(mxGetInf())) 12 | #define rtNaN (mxGetNaN()) 13 | #define rtIsNaN(X) ((int)mxIsNaN(X)) 14 | #define rtIsInf(X) ((int)mxIsInf(X)) 15 | 16 | /* Type Definitions */ 17 | 18 | /* Named Constants */ 19 | 20 | /* Variable Declarations */ 21 | extern uint32_T _QuadrotorMachineNumber_; 22 | extern real_T _sfTime_; 23 | 24 | /* Variable Definitions */ 25 | 26 | /* Function Declarations */ 27 | extern void Quadrotor_initializer(void); 28 | extern void Quadrotor_terminator(void); 29 | 30 | /* Function Definitions */ 31 | 32 | /* We load infoStruct for rtw_optimation_info on demand in mdlSetWorkWidths and 33 | free it immediately in mdlStart. Given that this is machine-wide as 34 | opposed to chart specific, we use NULL check to make sure it gets loaded 35 | and unloaded once per machine even though the methods mdlSetWorkWidths/mdlStart 36 | are chart/instance specific. The following methods abstract this out. */ 37 | extern mxArray* load_Quadrotor_optimization_info(void); 38 | extern void unload_Quadrotor_optimization_info(void); 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.lib -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.mak: -------------------------------------------------------------------------------- 1 | # ------------------- Required for MSVC nmake --------------------------------- 2 | # This file should be included at the top of a MAKEFILE as follows: 3 | 4 | 5 | CPU = AMD64 6 | !include 7 | 8 | MACHINE = Quadrotor 9 | TARGET = sfun 10 | CHART_SRCS = \ 11 | c1_Quadrotor.c\ 12 | c2_Quadrotor.c\ 13 | c3_Quadrotor.c 14 | MACHINE_SRC = Quadrotor_sfun.c 15 | MACHINE_REG = Quadrotor_sfun_registry.c 16 | MEX_WRAPPER = 17 | MAKEFILE = Quadrotor_sfun.mak 18 | MATLAB_ROOT = C:\Program Files\MATLAB\R2012a 19 | BUILDARGS = 20 | 21 | #--------------------------- Tool Specifications ------------------------------ 22 | # 23 | # 24 | MSVC_ROOT1 = $(MSDEVDIR:SharedIDE=vc) 25 | MSVC_ROOT2 = $(MSVC_ROOT1:SHAREDIDE=vc) 26 | MSVC_ROOT = $(MSVC_ROOT2:sharedide=vc) 27 | 28 | # Compiler tool locations, CC, LD, LIBCMD: 29 | CC = cl.exe 30 | LD = link.exe 31 | LIBCMD = lib.exe 32 | #------------------------------ Include/Lib Path ------------------------------ 33 | 34 | USER_INCLUDES = 35 | AUX_INCLUDES = 36 | ML_INCLUDES = /I "$(MATLAB_ROOT)\extern\include" 37 | SL_INCLUDES = /I "$(MATLAB_ROOT)\simulink\include" 38 | SF_INCLUDES = /I "C:\Program Files\MATLAB\R2012a\stateflow\c\mex\include" /I "C:\Program Files\MATLAB\R2012a\stateflow\c\debugger\include" 39 | 40 | DSP_INCLUDES = 41 | 42 | COMPILER_INCLUDES = /I "$(MSVC_ROOT)\include" 43 | 44 | INCLUDE_PATH = $(USER_INCLUDES) $(AUX_INCLUDES) $(ML_INCLUDES) $(SL_INCLUDES) $(SF_INCLUDES) $(DSP_INCLUDES) 45 | LIB_PATH = "$(MSVC_ROOT)\lib" 46 | 47 | CFLAGS = /c /Zp8 /GR /W3 /EHs /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DMATLAB_MEX_FILE /nologo /MD $(COMPFLAGS) 48 | LDFLAGS = /nologo /dll /OPT:NOREF /export:mexFunction 49 | AUXLDFLAGS = 50 | 51 | #----------------------------- Source Files ----------------------------------- 52 | 53 | REQ_SRCS = $(MACHINE_SRC) $(MACHINE_REG) $(MEX_WRAPPER) $(CHART_SRCS) 54 | 55 | USER_ABS_OBJS = 56 | 57 | AUX_ABS_OBJS = 58 | 59 | REQ_OBJS = $(REQ_SRCS:.cpp=.obj) 60 | REQ_OBJS2 = $(REQ_OBJS:.c=.obj) 61 | OBJS = $(REQ_OBJS2) $(USER_ABS_OBJS) $(AUX_ABS_OBJS) 62 | OBJLIST_FILE = Quadrotor_sfun.mol 63 | SFCLIB = "C:\Program Files\MATLAB\R2012a\stateflow\c\mex\lib\win64\sfc_mex.lib" "C:\Program Files\MATLAB\R2012a\stateflow\c\debugger\lib\win64\sfc_debug.lib" 64 | AUX_LNK_OBJS = 65 | USER_LIBS = 66 | LINK_MACHINE_LIBS = 67 | 68 | DSP_LIBS = 69 | BLAS_LIBS = "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmwblascompat32.lib" 70 | 71 | #--------------------------------- Rules -------------------------------------- 72 | 73 | MEX_FILE_NAME_WO_EXT = $(MACHINE)_$(TARGET) 74 | MEX_FILE_NAME = $(MEX_FILE_NAME_WO_EXT).mexw64 75 | MEX_FILE_CSF = 76 | all : $(MEX_FILE_NAME) $(MEX_FILE_CSF) 77 | 78 | MEXLIB = "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmx.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmex.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmat.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libfixedpoint.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libut.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmwmathutil.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libemlrt.lib" "C:\Program Files\MATLAB\R2012a\lib\win64\libippmwipt.lib" 79 | 80 | $(MEX_FILE_NAME) : $(MAKEFILE) $(OBJS) $(SFCLIB) $(AUX_LNK_OBJS) $(USER_LIBS) 81 | @echo ### Linking ... 82 | $(LD) $(LDFLAGS) $(AUXLDFLAGS) /OUT:$(MEX_FILE_NAME) /map:"$(MEX_FILE_NAME_WO_EXT).map" $(USER_LIBS) $(SFCLIB) $(AUX_LNK_OBJS) $(MEXLIB) $(LINK_MACHINE_LIBS) $(DSP_LIBS) $(BLAS_LIBS) @$(OBJLIST_FILE) 83 | mt -outputresource:"$(MEX_FILE_NAME);2" -manifest "$(MEX_FILE_NAME).manifest" 84 | @echo ### Created $@ 85 | 86 | .c.obj : 87 | @echo ### Compiling "$<" 88 | $(CC) $(CFLAGS) $(INCLUDE_PATH) "$<" 89 | 90 | .cpp.obj : 91 | @echo ### Compiling "$<" 92 | $(CC) $(CFLAGS) $(INCLUDE_PATH) "$<" 93 | 94 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.mexw64.manifest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.mol: -------------------------------------------------------------------------------- 1 | c1_Quadrotor.obj 2 | c2_Quadrotor.obj 3 | c3_Quadrotor.obj 4 | Quadrotor_sfun_registry.obj 5 | Quadrotor_sfun.obj 6 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun_registry.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun_registry.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c1_Quadrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef __c1_Quadrotor_h__ 2 | #define __c1_Quadrotor_h__ 3 | 4 | /* Include files */ 5 | #include "sfc_sf.h" 6 | #include "sfc_mex.h" 7 | #include "rtwtypes.h" 8 | 9 | /* Type Definitions */ 10 | typedef struct { 11 | const char * context; 12 | const char * name; 13 | const char * dominantType; 14 | const char * resolved; 15 | uint32_T fileTimeLo; 16 | uint32_T fileTimeHi; 17 | uint32_T mFileTimeLo; 18 | uint32_T mFileTimeHi; 19 | } c1_ResolvedFunctionInfo; 20 | 21 | typedef struct { 22 | int32_T c1_sfEvent; 23 | boolean_T c1_isStable; 24 | boolean_T c1_doneDoubleBufferReInit; 25 | uint8_T c1_is_active_c1_Quadrotor; 26 | SimStruct *S; 27 | ChartInfoStruct chartInfo; 28 | uint32_T chartNumber; 29 | uint32_T instanceNumber; 30 | real_T c1_Pp; 31 | boolean_T c1_Pp_not_empty; 32 | real_T c1_Q; 33 | boolean_T c1_Q_not_empty; 34 | real_T c1_R; 35 | boolean_T c1_R_not_empty; 36 | real_T c1_xp; 37 | boolean_T c1_xp_not_empty; 38 | real_T c1_A1; 39 | boolean_T c1_A1_not_empty; 40 | real_T c1_H; 41 | boolean_T c1_H_not_empty; 42 | real_T c1_I; 43 | boolean_T c1_I_not_empty; 44 | real_T c1_xp_dot; 45 | boolean_T c1_xp_dot_not_empty; 46 | } SFc1_QuadrotorInstanceStruct; 47 | 48 | /* Named Constants */ 49 | 50 | /* Variable Declarations */ 51 | 52 | /* Variable Definitions */ 53 | 54 | /* Function Declarations */ 55 | extern const mxArray *sf_c1_Quadrotor_get_eml_resolved_functions_info(void); 56 | 57 | /* Function Definitions */ 58 | extern void sf_c1_Quadrotor_get_check_sum(mxArray *plhs[]); 59 | extern void c1_Quadrotor_method_dispatcher(SimStruct *S, int_T method, void 60 | *data); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c1_Quadrotor.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c1_Quadrotor.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c2_Quadrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef __c2_Quadrotor_h__ 2 | #define __c2_Quadrotor_h__ 3 | 4 | /* Include files */ 5 | #include "sfc_sf.h" 6 | #include "sfc_mex.h" 7 | #include "rtwtypes.h" 8 | 9 | /* Type Definitions */ 10 | typedef struct { 11 | const char * context; 12 | const char * name; 13 | const char * dominantType; 14 | const char * resolved; 15 | uint32_T fileTimeLo; 16 | uint32_T fileTimeHi; 17 | uint32_T mFileTimeLo; 18 | uint32_T mFileTimeHi; 19 | } c2_ResolvedFunctionInfo; 20 | 21 | typedef struct { 22 | int32_T c2_sfEvent; 23 | boolean_T c2_isStable; 24 | boolean_T c2_doneDoubleBufferReInit; 25 | uint8_T c2_is_active_c2_Quadrotor; 26 | SimStruct *S; 27 | ChartInfoStruct chartInfo; 28 | uint32_T chartNumber; 29 | uint32_T instanceNumber; 30 | real_T c2_Pp; 31 | boolean_T c2_Pp_not_empty; 32 | real_T c2_Q; 33 | boolean_T c2_Q_not_empty; 34 | real_T c2_R; 35 | boolean_T c2_R_not_empty; 36 | real_T c2_xp; 37 | boolean_T c2_xp_not_empty; 38 | real_T c2_A1; 39 | boolean_T c2_A1_not_empty; 40 | real_T c2_H; 41 | boolean_T c2_H_not_empty; 42 | real_T c2_I; 43 | boolean_T c2_I_not_empty; 44 | real_T c2_xp_dot; 45 | boolean_T c2_xp_dot_not_empty; 46 | } SFc2_QuadrotorInstanceStruct; 47 | 48 | /* Named Constants */ 49 | 50 | /* Variable Declarations */ 51 | 52 | /* Variable Definitions */ 53 | 54 | /* Function Declarations */ 55 | extern const mxArray *sf_c2_Quadrotor_get_eml_resolved_functions_info(void); 56 | 57 | /* Function Definitions */ 58 | extern void sf_c2_Quadrotor_get_check_sum(mxArray *plhs[]); 59 | extern void c2_Quadrotor_method_dispatcher(SimStruct *S, int_T method, void 60 | *data); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c2_Quadrotor.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c2_Quadrotor.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c3_Quadrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef __c3_Quadrotor_h__ 2 | #define __c3_Quadrotor_h__ 3 | 4 | /* Include files */ 5 | #include "sfc_sf.h" 6 | #include "sfc_mex.h" 7 | #include "rtwtypes.h" 8 | 9 | /* Type Definitions */ 10 | typedef struct { 11 | const char * context; 12 | const char * name; 13 | const char * dominantType; 14 | const char * resolved; 15 | uint32_T fileTimeLo; 16 | uint32_T fileTimeHi; 17 | uint32_T mFileTimeLo; 18 | uint32_T mFileTimeHi; 19 | } c3_ResolvedFunctionInfo; 20 | 21 | typedef struct { 22 | int32_T c3_sfEvent; 23 | boolean_T c3_isStable; 24 | boolean_T c3_doneDoubleBufferReInit; 25 | uint8_T c3_is_active_c3_Quadrotor; 26 | SimStruct *S; 27 | ChartInfoStruct chartInfo; 28 | uint32_T chartNumber; 29 | uint32_T instanceNumber; 30 | real_T c3_Pp; 31 | boolean_T c3_Pp_not_empty; 32 | real_T c3_Q; 33 | boolean_T c3_Q_not_empty; 34 | real_T c3_R; 35 | boolean_T c3_R_not_empty; 36 | real_T c3_xp; 37 | boolean_T c3_xp_not_empty; 38 | real_T c3_A1; 39 | boolean_T c3_A1_not_empty; 40 | real_T c3_H; 41 | boolean_T c3_H_not_empty; 42 | real_T c3_I; 43 | boolean_T c3_I_not_empty; 44 | real_T c3_xp_dot; 45 | boolean_T c3_xp_dot_not_empty; 46 | } SFc3_QuadrotorInstanceStruct; 47 | 48 | /* Named Constants */ 49 | 50 | /* Variable Declarations */ 51 | 52 | /* Variable Definitions */ 53 | 54 | /* Function Declarations */ 55 | extern const mxArray *sf_c3_Quadrotor_get_eml_resolved_functions_info(void); 56 | 57 | /* Function Definitions */ 58 | extern void sf_c3_Quadrotor_get_check_sum(mxArray *plhs[]); 59 | extern void c3_Quadrotor_method_dispatcher(SimStruct *S, int_T method, void 60 | *data); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c3_Quadrotor.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/c3_Quadrotor.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/mexopts.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | rem MSVC100OPTS.BAT 3 | rem 4 | rem Compile and link options used for building MEX-files 5 | rem using the Microsoft Visual C++ compiler version 10.0 6 | rem 7 | rem $Revision: 1.1.6.3 $ $Date: 2011/03/09 05:35:31 $ 8 | rem Copyright 2007-2009 The MathWorks, Inc. 9 | rem 10 | rem StorageVersion: 1.0 11 | rem C++keyFileName: MSVC100OPTS.BAT 12 | rem C++keyName: Microsoft Visual C++ 2010 13 | rem C++keyManufacturer: Microsoft 14 | rem C++keyVersion: 10.0 15 | rem C++keyLanguage: C++ 16 | rem 17 | rem ******************************************************************** 18 | rem General parameters 19 | rem ******************************************************************** 20 | 21 | set MATLAB=%MATLAB% 22 | set VSINSTALLDIR=c:\Program Files (x86)\Microsoft Visual Studio 10.0 23 | set VCINSTALLDIR=%VSINSTALLDIR%\VC 24 | rem In this case, LINKERDIR is being used to specify the location of the SDK 25 | set LINKERDIR=C:\Program Files (x86)\Microsoft SDKs\Windows\v7.0A\ 26 | set PATH=%VCINSTALLDIR%\bin\amd64;%VCINSTALLDIR%\bin;%VCINSTALLDIR%\VCPackages;%VSINSTALLDIR%\Common7\IDE;%VSINSTALLDIR%\Common7\Tools;%LINKERDIR%\bin\x64;%LINKERDIR%\bin;%MATLAB_BIN%;%PATH% 27 | set INCLUDE=%VCINSTALLDIR%\INCLUDE;%VCINSTALLDIR%\ATLMFC\INCLUDE;%LINKERDIR%\include;%INCLUDE% 28 | set LIB=%VCINSTALLDIR%\LIB\amd64;%VCINSTALLDIR%\ATLMFC\LIB\amd64;%LINKERDIR%\lib\x64;%MATLAB%\extern\lib\win64;%LIB% 29 | set MW_TARGET_ARCH=win64 30 | 31 | rem ******************************************************************** 32 | rem Compiler parameters 33 | rem ******************************************************************** 34 | set COMPILER=cl 35 | set COMPFLAGS=/c /GR /W3 /EHs /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DMATLAB_MEX_FILE /nologo /MD 36 | set OPTIMFLAGS=/O2 /Oy- /DNDEBUG 37 | set DEBUGFLAGS=/Z7 38 | set NAME_OBJECT=/Fo 39 | 40 | rem ******************************************************************** 41 | rem Linker parameters 42 | rem ******************************************************************** 43 | set LIBLOC=%MATLAB%\extern\lib\win64\microsoft 44 | set LINKER=link 45 | set LINKFLAGS=/dll /export:%ENTRYPOINT% /LIBPATH:"%LIBLOC%" libmx.lib libmex.lib libmat.lib /MACHINE:X64 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /manifest /incremental:NO /implib:"%LIB_NAME%.x" /MAP:"%OUTDIR%%MEX_NAME%%MEX_EXT%.map" 46 | set LINKOPTIMFLAGS= 47 | set LINKDEBUGFLAGS=/debug /PDB:"%OUTDIR%%MEX_NAME%%MEX_EXT%.pdb" 48 | set LINK_FILE= 49 | set LINK_LIB= 50 | set NAME_OUTPUT=/out:"%OUTDIR%%MEX_NAME%%MEX_EXT%" 51 | set RSP_FILE_INDICATOR=@ 52 | 53 | rem ******************************************************************** 54 | rem Resource compiler parameters 55 | rem ******************************************************************** 56 | set RC_COMPILER=rc /fo "%OUTDIR%mexversion.res" 57 | set RC_LINKER= 58 | 59 | set POSTLINK_CMDS=del "%LIB_NAME%.x" "%LIB_NAME%.exp" 60 | set POSTLINK_CMDS1=mt -outputresource:"%OUTDIR%%MEX_NAME%%MEX_EXT%;2" -manifest "%OUTDIR%%MEX_NAME%%MEX_EXT%.manifest" 61 | set POSTLINK_CMDS2=del "%OUTDIR%%MEX_NAME%%MEX_EXT%.manifest" 62 | set POSTLINK_CMDS3=del "%OUTDIR%%MEX_NAME%%MEX_EXT%.map" 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/rtwtypeschksum.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D + LIDAR/slprj/_sfprj/Quadrotor/_self/sfun/src/rtwtypeschksum.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/1.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Airmodel.m: -------------------------------------------------------------------------------- 1 | global A 2 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Forces.m: -------------------------------------------------------------------------------- 1 | function Forces 2 | global A 3 | % this function will calculates the forces and torques on the quadrotor 4 | % give the speed of each propeler. 5 | 6 | % Inputs 7 | % O1 = Front (CCW) 8 | % O2 = Right (CW) 9 | % O3 = Rear (CCW) 10 | % O4 = Left (CW) 11 | 12 | % Outputs 13 | % U1 = Throttle 14 | % U2 = Roll movement 15 | % U3 = Pitch movement 16 | % U4 = Yaw movement 17 | 18 | % Constants 19 | % l = Where l [m] is the distance between the center of the quadrotor and 20 | % the center of a propeller. 21 | % b = 22 | 23 | % A.U11_plot(A.counter) = A.U1; 24 | A.U1 = A.b*(sign(A.O1)*A.O1^2 + sign(A.O2)*A.O2^2 + sign(A.O3)*A.O3^2 + sign(A.O4)*A.O4^2); 25 | A.U1_plot(A.counter) = A.U1; 26 | 27 | % A.U21_plot(A.counter) = A.U1; 28 | A.U2 = A.b*A.l*(sign(A.O4)*A.O4^2 - sign(A.O2)*A.O2^2); 29 | A.U2_plot(A.counter) = A.U2; 30 | 31 | % A.U31_plot(A.counter) = A.U1; 32 | A.U3 = A.b*A.l*(sign(A.O3)*A.O3^2 - sign(A.O1)*A.O1^2); 33 | A.U3_plot(A.counter) = A.U3; 34 | 35 | % A.U41_plot(A.counter) = A.U1; 36 | A.U4 = A.d*(sign(A.O2)*A.O2^2 + sign(A.O4)*A.O4^2 - sign(A.O1)*A.O1^2 - sign(A.O3)*A.O3^2); 37 | A.U4_plot(A.counter) = A.U4; 38 | 39 | A.O = (-A.O1 + A.O2 - A.O3 + A.O4); 40 | A.O_plot(A.counter) = A.O; 41 | 42 | 43 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Go1.m: -------------------------------------------------------------------------------- 1 | global A -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/IMU_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter); %erros is +- 2 mrad 5 | A.theta_meas = A.theta+A.theta_error(A.counter); %erros is +- 2 mrad 6 | A.psi_meas = A.psi+A.psi_error(A.counter); %erros is +- 2 mrad 7 | 8 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/Kalman.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_X.m: -------------------------------------------------------------------------------- 1 | function Kalman_X 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (A.U1/A.m*A.theta_kalman - (A.X_dot*cos(A.psi_kalman)+A.Y_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_X2.m: -------------------------------------------------------------------------------- 1 | function Kalman_X2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((sin(A.psi_kalman)*sin(A.phi_kalman) + cos(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.X_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.X_kalman = xp+K*(A.X_meas-H*xp); 15 | 16 | A.X_kalman_plot(A.counter) = A.X_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.X_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_Y.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.U1/A.m*A.phi_kalman - (A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman))*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | A.Y_kalman 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_Y2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Y2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = ((-cos(A.psi_kalman)*sin(A.phi_kalman) + sin(A.psi_kalman)*sin(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m - A.Y_dot*1.2)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Y_kalman = xp+K*(A.Y_meas-H*xp); 15 | 16 | A.Y_kalman_plot(A.counter) = A.Y_kalman; 17 | % Update covariance 18 | Pp=(I-K*H)*Pp; 19 | xp = A.Y_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_Z.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_Z2.m: -------------------------------------------------------------------------------- 1 | function Kalman_Z2 2 | global A 3 | persistent Pp Q R xp A1 H I xp_dot 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); xp_dot = 0; 7 | end 8 | % Compute priori 9 | xp_dot = (-A.g + (cos(A.theta_kalman)*cos(A.phi_kalman))*A.U1/A.m)*A.Ts + xp_dot; 10 | xp=xp_dot*A.Ts + xp; 11 | Pp=A1*Pp*A1'+Q; 12 | % Measurement update 13 | K = Pp*H'*inv(H*Pp*H'+R); 14 | A.Z_kalman = xp+K*(A.Z_meas-H*xp); 15 | A.Z_kalman_plot(A.counter) = A.Z_kalman; 16 | % Update covariance 17 | Pp=(I-K*H)*Pp; 18 | xp = A.Z_kalman; 19 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_function1.m: -------------------------------------------------------------------------------- 1 | function xhat = Kalman_function1(z,xpp,RR,QQ) 2 | persistent Pp Q R xp A H I 3 | if isempty(Pp) 4 | Pp=1; Q=QQ; R=RR; 5 | xp=xpp; A=0.5; H=1; I=eye(1); 6 | end 7 | % Compute priori 8 | xp=A*xp; 9 | Pp=A*Pp*A'+Q; 10 | % Measurement update 11 | K = Pp*H'*inv(H*Pp*H'+R); 12 | xhat = xp+K*(z-H*xp); 13 | % Update covariance 14 | Pp=(I-K*H)*Pp; 15 | xp = xhat; 16 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_phi.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.p*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_phi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_phi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(A.p + sin(A.phi_kalman)*tan(A.theta_kalman)*A.q + cos(A.phi_kalman)*tan(A.theta_kalman)*A.r)*A.Ts + xp; 10 | % Pp=A1*Pp*A1'+Q 11 | % Pp = 6.5331; 12 | % Measurement update 13 | % K = Pp*H'*inv(H*Pp*H'+R) 14 | K = 0.0613; 15 | A.phi_kalman = xp+K*(A.phi_meas-xp); 16 | A.phi_kalman_plot(A.counter) = A.phi_kalman; 17 | % Update covariance 18 | % Pp=(I-K*H)*Pp 19 | xp = A.phi_kalman; 20 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_psi.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.r*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_psi2.m: -------------------------------------------------------------------------------- 1 | function Kalman_psi2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(sin(A.phi_kalman)/cos(A.theta_kalman)*A.q + cos(A.phi_kalman)/cos(A.theta_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.psi_kalman = xp+K*(A.psi_meas-H*xp); 14 | A.psi_kalman_plot(A.counter) = A.psi_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.psi_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_theta.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=A.q*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalman_theta2.m: -------------------------------------------------------------------------------- 1 | function Kalman_theta2 2 | global A 3 | persistent Pp Q R xp A1 H I 4 | if isempty(Pp) 5 | Pp=1; Q=5; R=100; 6 | xp=0; A1=0.5; H=1; I=eye(1); 7 | end 8 | % Compute priori 9 | xp=(cos(A.phi_kalman)*A.q - sin(A.phi_kalman)*A.r)*A.Ts + xp; 10 | Pp=A1*Pp*A1'+Q; 11 | % Measurement update 12 | K = Pp*H'*inv(H*Pp*H'+R); 13 | A.theta_kalman = xp+K*(A.theta_meas-H*xp); 14 | A.theta_kalman_plot(A.counter) = A.theta_kalman; 15 | % Update covariance 16 | Pp=(I-K*H)*Pp; 17 | xp = A.theta_kalman; 18 | % End or function -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Kalmanfilter_prob1_sfun.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/Kalmanfilter_prob1_sfun.mexw64 -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/LIDAR_Plot.m: -------------------------------------------------------------------------------- 1 | 2 | global A 3 | 4 | % simLIDARinitial; 5 | % A.LIDARBeams = 4*ones(181,1) 6 | 7 | A.X22 = A.LIDARBeams.*cos(A.theta11) + A.LIDAR_X_offset; 8 | A.Y22 = A.LIDARBeams.*sin(A.theta11) + A.LIDAR_Y_offset; 9 | A.Z22 = zeros(1,181) + A.LIDAR_Z_offset; 10 | % 11 | % A.theta = 0*45*pi/180; 12 | % A.phi = 0*45*pi/180; 13 | % A.psi = 45*pi/180; 14 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X22,A.Y22,A.Z22,A.phi,A.theta,A.psi); 15 | % Z 16 | set(A.LIDAR_Patch,'xdata',A.X22'+A.X,'ydata',A.Y22'+A.Y,'zdata',A.Z22+A.Z) 17 | 18 | % axis([-4 4 -4 4 -4 4]) 19 | 20 | % grid on 21 | % view(50,50) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Map.m: -------------------------------------------------------------------------------- 1 | global A 2 | % imshow('AUS.jpg') 3 | A.aus = imread('AUS.jpg'); 4 | axis equal 5 | % [x,y,z] = sphere(50); 6 | A.t1=-250:1:249; 7 | A.c1=-250:1:249; 8 | A.x1=[]; 9 | A.y1=[]; 10 | A.z1=zeros(500,500); 11 | A.b=0; 12 | while A.b<500; 13 | A.x1=[A.x1;A.c1]; 14 | A.y1=[A.y1;A.t1]; 15 | A.b = A.b+1; 16 | end 17 | A.y1=A.y1'; 18 | A.props.AmbientStrength = 0.1; 19 | A.props.DiffuseStrength = 1; 20 | A.props.SpecularColorReflectance = .5; 21 | A.props.SpecularExponent = 20; 22 | A.props.SpecularStrength = 1; 23 | A.props.FaceColor= 'texture'; 24 | A.props.EdgeColor = 'none'; 25 | A.props.FaceLighting = 'phong'; 26 | A.props.Cdata = A.aus; 27 | surface(A.x1,A.y1,A.z1,A.props); -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Motors_Speed.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if abs(BB1)>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if abs(BB2)>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if abs(BB3)>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if abs(BB4)>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | A.O1 = sign(BB1)*sqrt(abs(BB1)); % Front M 26 | A.O2 = sign(BB2)*sqrt(abs(BB2)); % Right M 27 | A.O3 = sign(BB3)*sqrt(abs(BB3)); % Rear M 28 | A.O4 = sign(BB4)*sqrt(abs(BB4)); % Left M 29 | 30 | A.O1_plot(A.counter) = A.O1; 31 | A.O2_plot(A.counter) = A.O2; 32 | A.O3_plot(A.counter) = A.O3; 33 | A.O4_plot(A.counter) = A.O4; 34 | 35 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Motors_Speed_2.m: -------------------------------------------------------------------------------- 1 | function Motors_Speed_2 2 | global A 3 | 4 | BB1 = A.U1/(4*A.b) - A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 5 | BB2 = A.U1/(4*A.b) - A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 6 | BB3 = A.U1/(4*A.b) + A.U3/(2*A.b*A.l) - A.U4/(4*A.d); 7 | BB4 = A.U1/(4*A.b) + A.U2/(2*A.b*A.l) + A.U4/(4*A.d); 8 | 9 | if BB1>A.Motors_limit 10 | BB1 = sign(BB1)*A.Motors_limit; 11 | end 12 | 13 | if BB2>A.Motors_limit 14 | BB2 = sign(BB2)*A.Motors_limit; 15 | end 16 | 17 | if BB3>A.Motors_limit 18 | BB3 = sign(BB3)*A.Motors_limit; 19 | end 20 | 21 | if BB4>A.Motors_limit 22 | BB4 = sign(BB4)*A.Motors_limit; 23 | end 24 | 25 | if BB1 pi/4) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/4; 13 | end 14 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_X_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.X_des = A.X_des_EF*cos(A.psi_meas)+A.Y_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | 5 | A.X_BF = A.X_meas*cos(A.psi_meas)+A.Y_meas*sin(A.psi_meas); % calculating X in BF 6 | A.X_dot_BF = A.X_dot*cos(A.psi_meas)+A.Y_dot*sin(A.psi_meas); % calculating X_dot in BF 7 | 8 | % PD controller for X_position 9 | A.theta_des = A.X_KP*(A.X_des - A.X_BF) + A.X_KD*A.X_dot; 10 | 11 | if(abs(A.theta_des) > pi/18) % limiter 12 | A.theta_des = sign(A.theta_des)*pi/18; 13 | end 14 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_Y.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_kalman)-A.X_des_EF*sin(A.psi_kalman); % calculating the desired X in BF 4 | A.Y_BF = A.Y_kalman*cos(A.psi_kalman)-A.X_kalman*sin(A.psi_kalman); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_kalman)-A.X_dot*sin(A.psi_kalman); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/4) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/4; 12 | end 13 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_Y_NC.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | A.Y_des = A.Y_des_EF*cos(A.psi_meas)-A.X_des_EF*sin(A.psi_meas); % calculating the desired X in BF 4 | A.Y_BF = A.Y_meas*cos(A.psi_meas)-A.X_meas*sin(A.psi_meas); % calculating X in BF 5 | A.Y_dot_BF = A.Y_dot*cos(A.psi_meas)-A.X_dot*sin(A.psi_meas); % calculating X_dot in BF 6 | 7 | % PD controller for X_position 8 | A.phi_des = -1*(A.Y_KP*(A.Y_des - A.Y_BF) + A.Y_KD*A.Y_dot); 9 | 10 | if(abs(A.phi_des) > pi/18) % limiter 11 | A.phi_des = sign(A.phi_des)*pi/18; 12 | end 13 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_Z.m: -------------------------------------------------------------------------------- 1 | function PID_Z 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_kalman; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_kalman)/cos(A.theta_kalman); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_Z_NC.m: -------------------------------------------------------------------------------- 1 | function PID_Z_NC 2 | global A 3 | persistent ui; 4 | persistent errork1; 5 | persistent vhatk1; 6 | % initialize persistent variables at beginning of simulation 7 | if A.init==0 8 | ui = 0; 9 | errork1 = 0; 10 | vhatk1 = 0; 11 | end 12 | 13 | error = A.Z_des-A.Z_meas; 14 | % proportional term1 15 | up = A.Z_KP * error; 16 | 17 | % integral term 18 | ui = ui + A.Z_KI * A.Ts/2 * (error + errork1); 19 | 20 | % dirty derivative of pe to get vhat 21 | vhat = (2*A.tau-A.Ts)/(2*A.tau+A.Ts)*vhatk1 + (2/(2*A.tau+A.Ts))*(error - errork1); 22 | ud = A.Z_KD*vhat; 23 | 24 | F = up + ui + ud; 25 | 26 | A.U1 = A.m*(A.g + F)/cos(A.phi_meas)/cos(A.theta_meas); 27 | A.U1_plot(A.counter) = A.U1; 28 | 29 | % update stored variables 30 | errork1 = error; 31 | vhatk1 = vhat; 32 | 33 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_heading.m: -------------------------------------------------------------------------------- 1 | function PID_heading 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_kalman ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_heading_NC.m: -------------------------------------------------------------------------------- 1 | function PID_heading_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | error = A.psi_des - A.psi_meas ; 12 | 13 | % proportional term 14 | up = A.psi_KP * error; 15 | 16 | % integral term 17 | ui = uik1 + A.psi_KI * A.Ts/2 * (error + errork1); 18 | 19 | % derivative term 20 | ud = A.psi_KD*A.r; 21 | 22 | 23 | % implement PID control 24 | A.U4 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_pitch.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_kalman; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_pitch_NC.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % pitch_attitude_hold 3 | % - regulate pitch attitude 4 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5 | function PID_pitch_NC 6 | global A 7 | persistent uik1; 8 | persistent errork1; 9 | % initialize persistent variables at beginning of simulation 10 | if A.init==0, 11 | uik1 = 0; 12 | errork1 = 0; 13 | end 14 | 15 | % error equation 16 | error = A.theta_des - A.theta_meas; 17 | 18 | % proportional term 19 | up = A.theta_KP * error; 20 | 21 | % integral term 22 | ui = uik1 + A.theta_KI * A.Ts/2 * (error + errork1); 23 | 24 | % derivative term 25 | ud = A.theta_KD*A.q; 26 | 27 | 28 | % implement PID control 29 | A.U3 = up + ui + ud; 30 | 31 | % update stored variables 32 | uik1 = ui; 33 | errork1 = error; 34 | 35 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_roll.m: -------------------------------------------------------------------------------- 1 | function PID_roll 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_kalman; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/PID_roll_NC.m: -------------------------------------------------------------------------------- 1 | function PID_roll_NC 2 | global A 3 | persistent uik1; 4 | persistent errork1; 5 | % initialize persistent variables at beginning of simulation 6 | if A.init==0, 7 | uik1 = 0; 8 | errork1 = 0; 9 | end 10 | 11 | % error equation 12 | error = A.phi_des - A.phi_meas; 13 | 14 | % proportional term 15 | up = A.phi_KP * error; 16 | 17 | % integral term 18 | ui = uik1 + A.phi_KI * A.Ts/2 * (error + errork1); 19 | 20 | % derivative term 21 | ud = A.phi_KD*A.p; 22 | 23 | % implement PID control 24 | A.U2 = up + ui + ud; 25 | 26 | % update stored variables 27 | uik1 = ui; 28 | errork1 = error; -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Path_manager.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | % hold on 4 | % if(A.init==0) 5 | % plot3(A.X_path,A.Y_path,A.Z_path,'+r','linewidth',3) 6 | % end 7 | 8 | if(A.path_counter <= length(A.X_path)) 9 | A.X_des_EF = A.X_path(A.path_counter); 10 | A.Y_des_EF = A.Y_path(A.path_counter); 11 | A.Z_des = A.Z_path(A.path_counter); 12 | else 13 | plot_XY; 14 | A.flagggg=1; 15 | end 16 | % A.X_des_EF 17 | % A.Y_des_EF 18 | % A.path_counter 19 | % A.X_kalman 20 | % abs(A.X_kalman-A.X_des)<.02 21 | if((abs(A.X_kalman-A.X_des_EF)<.04)&&(abs(A.Y_kalman-A.Y_des_EF)<.04)&&(abs(A.Z_kalman-A.Z_des)<.08)) 22 | A.path_counter = A.path_counter+1; 23 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Quadrotor.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/Quadrotor.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Quadrotor_sfun.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/Quadrotor_sfun.mexw64 -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/READ ME.txt: -------------------------------------------------------------------------------- 1 | Run script2run.m 2 | 3 | Enjoy :) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/SimInitial.m: -------------------------------------------------------------------------------- 1 | function SimInitial 2 | global A 3 | % 4 | % A.X1wall = 0; % X first point in wall NF were X is on the right direction and Y is in front !! 5 | % A.Y1wall = 1; % Y first point in wall 6 | % A.X2wall = .5; % X second point in wall X2 has to be > X1 7 | % A.Y2wall = 1; % Y second point in wall 8 | % 9 | % A.Z1wall = 0; % Z lower edge 10 | % A.Z2wall = 1; % Z higher edge 11 | 12 | % D1 B1 C1 are three points on the wall that form a plane [X,Y,Z] 13 | % the ordering of the points will be like this 14 | % B1 = [X1,Y1,Z1] 15 | % C1 = [X2,Y2,Z1] 16 | % D1 = [(X1+X2)/2,(Y1+Y2)/2,Z2] 17 | 18 | % obstacle num 1 19 | A.B1(1,:) = [-1,1,0]; 20 | A.C1(1,:) = [1,1,0]; 21 | A.D1(1,:) = [.5,1,1]; 22 | 23 | %obstacle num 2 24 | A.B1(2,:) = [0,2,0]; 25 | A.C1(2,:) = [2,2,0]; 26 | A.D1(2,:) = [1,2,2]; 27 | 28 | %obstacle num 3 29 | A.B1(3,:) = [-2,1,0]; 30 | A.C1(3,:) = [-1,3,0]; 31 | A.D1(3,:) = [-1.5,2,1.5]; 32 | 33 | A.num_obstacles = size(A.D1); 34 | 35 | for i=1:A.num_obstacles(1) 36 | 37 | patch([A.B1(i,1) A.C1(i,1) A.C1(i,1) A.B1(i,1)],[A.B1(i,2) A.C1(i,2) A.C1(i,2) A.B1(i,2)],[A.B1(i,3) A.B1(i,3) A.D1(i,3) A.D1(i,3)],[0 .4 1]) 38 | 39 | DB = A.B1(i,:)-A.D1(i,:); 40 | DC = A.C1(i,:)-A.D1(i,:); 41 | 42 | A.M1(i) = DB(2)*DC(3) - DB(3)*DC(2); 43 | A.M2(i) = DB(1)*DC(3) - DB(3)*DC(1); 44 | A.M3(i) = DB(1)*DC(2) - DB(2)*DC(1); 45 | end 46 | 47 | A.X1 = 0; % quadrotors position X 48 | A.Y1 = 0; % quadrotors position Y 49 | A.Z1 = .5; % quadrotors position Z 50 | 51 | A.theta11 = [0:pi/180:pi]; % used in ploting the LIDAR beams 52 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/SimInitial2.m: -------------------------------------------------------------------------------- 1 | function SimInitial2 2 | global A 3 | % 4 | % A.X1wall = 0; % X first point in wall NF were X is on the right direction and Y is in front !! 5 | % A.Y1wall = 1; % Y first point in wall 6 | % A.X2wall = .5; % X second point in wall X2 has to be > X1 7 | % A.Y2wall = 1; % Y second point in wall 8 | % 9 | % A.Z1wall = 0; % Z lower edge 10 | % A.Z2wall = 1; % Z higher edge 11 | 12 | % D1 B1 C1 are three points on the wall that form a plane [X,Y,Z] 13 | % the ordering of the points will be like this 14 | % B1 = [X1,Y1,Z1] 15 | % C1 = [X2,Y2,Z1] 16 | % D1 = [(X1+X2)/2,(Y1+Y2)/2,Z2] 17 | 18 | % samll room obstacle 19 | Xi = [-1.5 -1.5 .5 -1.5 -1.5 -1.5 3 1.5 1.5]; 20 | Xf = [1.5 -.5 1.5 1.5 -1.5 3 3 3 1.5]; 21 | Yi = [1 1 1 1 1 5 2 2 1]; 22 | Yf = [1 1 1 1 5 5 5 2 2]; 23 | Zi = [0 .5 .5 1 0 0 0 0 0]; 24 | Zf = [.5 1 1 1.5 1.5 1.5 1.5 1.5 1.5]; 25 | 26 | 27 | % random wall 28 | Xi = [-1.5 -3 -1]; 29 | Xf = [1 -2.5 1.5]; 30 | Yi = [1 -.5 2.2]; 31 | Yf = [1 2 2.2]; 32 | Zi = [0 0 0]; 33 | Zf = [1.5 2.5 3]; 34 | 35 | for i=1:length(Xi) 36 | [A.B1(i,:),A.C1(i,:),A.D1(i,:)] = obstacle([Xi(i) Xf(i)],[Yi(i) Yf(i)],[Zi(i) Zf(i)]); 37 | % [A.B1(2,:),A.C1(2,:),A.D1(2,:)] = obstacle([-1.5 -.5],[1 1],[.5 1]); 38 | end 39 | 40 | % A.B1(1,:) = [-1,1,0]; 41 | % A.C1(1,:) = [1,1,0]; 42 | % A.D1(1,:) = [.5,1,1]; 43 | 44 | %obstacle num 2 45 | % A.B1(2,:) = [0,2,0]; 46 | % A.C1(2,:) = [2,2,0]; 47 | % A.D1(2,:) = [1,2,2]; 48 | 49 | %obstacle num 3 50 | % A.B1(3,:) = [-2,1,0]; 51 | % A.C1(3,:) = [-1,3,0]; 52 | % A.D1(3,:) = [-1.5,2,1.5]; 53 | 54 | A.num_obstacles = size(A.D1); 55 | 56 | for i=1:A.num_obstacles(1) 57 | 58 | patch([A.B1(i,1) A.C1(i,1) A.C1(i,1) A.B1(i,1)],[A.B1(i,2) A.C1(i,2) A.C1(i,2) A.B1(i,2)],[A.B1(i,3) A.B1(i,3) A.D1(i,3) A.D1(i,3)],[0 .4 1]) 59 | 60 | DB = A.B1(i,:)-A.D1(i,:); 61 | DC = A.C1(i,:)-A.D1(i,:); 62 | 63 | A.M1(i) = DB(2)*DC(3) - DB(3)*DC(2); 64 | A.M2(i) = DB(1)*DC(3) - DB(3)*DC(1); 65 | A.M3(i) = DB(1)*DC(2) - DB(2)*DC(1); 66 | end 67 | 68 | A.X1 = 0; % quadrotors position X 69 | A.Y1 = 0; % quadrotors position Y 70 | A.Z1 = .5; % quadrotors position Z 71 | 72 | A.theta11 = [0:pi/180:pi]; % used in ploting the LIDAR beams 73 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Sim_LIDAR.m: -------------------------------------------------------------------------------- 1 | function Sim_LIDAR 2 | 3 | global A 4 | % 5 | A.X1 = A.X + A.LIDAR_X_offset; 6 | A.Y1 = A.Y + A.LIDAR_Y_offset; 7 | A.Z1 = A.Z + A.LIDAR_Z_offset; 8 | 9 | % A.X1 = A.X; 10 | % A.Y1 = A.Y; 11 | % A.Z1 = A.Z; 12 | 13 | tic 14 | 15 | [A.X2,A.Y2,A.Z2]=rotateXYZ(A.X2_init,A.Y2_init,A.Z2_init,A.phi,A.theta,A.psi); 16 | A.Z2 = A.Z2+A.Z1; 17 | A.X2 = A.X2+A.X1; 18 | A.Y2 = A.Y2+A.Y1; 19 | 20 | Beam_distances = 4*ones(1,181); 21 | Beam_distances_ground = 4*ones(1,181); 22 | A.LIDARBeams = 4*ones(1,181); 23 | %--------------- Obstacles --------------------% 24 | for i=1:A.num_obstacles(1) 25 | 26 | m = -1*(A.M1(i)*(A.X1*ones(1,181)-A.D1(i,1)) - A.M2(i)*(A.Y1*ones(1,181)-A.D1(i,2)) + A.M3(i)*(A.Z1*ones(1,181)-A.D1(i,3)))./(A.M1(i)*(A.X2-A.X1) - A.M2(i)*(A.Y2-A.Y1) + A.M3(i)*(A.Z2-A.Z1)); 27 | 28 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 29 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 30 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 31 | 32 | % check if the intersection is in the same direction of them beam or not 33 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 34 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 35 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 36 | 37 | % intersection with the obstacle 38 | BI5 = (X_suggested>=A.B1(i,1)-eps*10); 39 | BI6 = (X_suggested<=A.C1(i,1)+eps*10); 40 | BI7 = (Y_suggested>=A.B1(i,2)-eps*10); 41 | BI8 = (Y_suggested<=A.C1(i,2)+eps*10); 42 | BI9 = (Z_suggested>=A.B1(i,3)-eps*10); 43 | BI10 = (Z_suggested<=A.D1(i,3)+eps*10); 44 | 45 | BI = ((BI1&BI2&BI3)&BI5&BI6&BI7&BI8&BI9&BI10); 46 | Beam_distances(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 47 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances); 48 | end 49 | %------------------ Ground ------------------------% 50 | m = -1*A.Z1*ones(1,181)./((A.Z2-A.Z1)); 51 | 52 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 53 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 54 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 55 | 56 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 57 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 58 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 59 | BI = (BI1&BI2&BI3); 60 | 61 | Beam_distances_ground(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 62 | 63 | % A.LIDARBeams = min(A.LIDARBeams,Beam_distances); 64 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances_ground); 65 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Sim_LIDAR_2.m: -------------------------------------------------------------------------------- 1 | function Sim_LIDAR_2 2 | 3 | global A 4 | 5 | A.X1 = A.X; 6 | A.Y1 = A.Y; 7 | A.Z1 = A.Z; 8 | 9 | tic 10 | 11 | [A.X2,A.Y2,A.Z2]=rotateXYZ(A.X2_init,A.Y2_init,A.Z2_init,A.theta,A.phi,A.psi); 12 | A.Z2 = A.Z2+A.Z1; 13 | A.X2 = A.X2+A.X1; 14 | A.Y2 = A.Y2+A.Y1; 15 | 16 | Beam_distances = 4*ones(1,181); 17 | Beam_distances_ground = 4*ones(1,181); 18 | A.LIDARBeams = 4*ones(1,181); 19 | %--------------- Obstacles -------------------- 20 | m = -1*(A.M1*(A.X1*ones(1,181)-A.D1(1)) - A.M2*(A.Y1*ones(1,181)-A.D1(2)) + A.M3*(A.Z1*ones(1,181)-A.D1(3)))./(A.M1*(A.X2-A.X1) - A.M2*(A.Y2-A.Y1) + A.M3*(A.Z2-A.Z1)); 21 | 22 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 23 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 24 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 25 | 26 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 27 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 28 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 29 | 30 | BI5 = (X_suggested>=-1); 31 | BI6 = (X_suggested<=1); 32 | BI7 = (Y_suggested>=0); 33 | BI8 = (Y_suggested<=2); 34 | BI9 = (Z_suggested>=0); 35 | BI10 = (Z_suggested<=(Y_suggested/2)); 36 | 37 | BI = ((BI1&BI2&BI3)&BI5&BI6&BI7&BI8&BI9&BI10); 38 | Beam_distances(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 39 | 40 | 41 | %------------------ Ground ------------------------% 42 | m = -1*A.Z1*ones(1,181)./((A.Z2-A.Z1)); 43 | 44 | X_suggested = (A.X1 + (A.X2 - A.X1).*m); 45 | Y_suggested = (A.Y1 + (A.Y2 - A.Y1).*m); 46 | Z_suggested = (A.Z1 + (A.Z2 - A.Z1).*m); 47 | 48 | BI1 = (sign(A.X2-A.X1)==sign(X_suggested-A.X1)); 49 | BI2 = (sign(A.Y2-A.Y1)==sign(Y_suggested-A.Y1)); 50 | BI3 = (sign(A.Z2-A.Z1)==sign(Z_suggested-A.Z1)); 51 | BI = (BI1&BI2&BI3); 52 | 53 | Beam_distances_ground(BI) = sqrt((X_suggested(BI)-A.X1).^2 + (Y_suggested(BI)-A.Y1).^2 + (Z_suggested(BI)-A.Z1).^2); 54 | 55 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances); 56 | A.LIDARBeams = min(A.LIDARBeams,Beam_distances_ground); 57 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/XY_meas.m: -------------------------------------------------------------------------------- 1 | function XY_meas 2 | global A 3 | 4 | A.X_meas = A.X+A.X_error(A.counter); %erros is +- 1 cm 5 | A.Y_meas = A.Y+A.Y_error(A.counter); %erros is +- 1 cm 6 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Y_Backstepping.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | if(A.U1~=0) 4 | A.U2 = .1*(-5*(A.Y - A.Y_des) -10*A.Y_dot - 9*A.U1*A.S_psi*A.C_phi ... 5 | -4*A.U1*A.psi_dot*A.C_psi*A.C_phi + A.U1*A.psi_dot^2*A.S_psi*A.C_phi ... 6 | +2*A.U1*A.psi_dot*A.S_psi*A.S_phi + A.U1*A.psi_dot*A.phi_dot*A.C_psi*A.S_phi ... 7 | -A.U1*A.phi_dot*A.psi_dot*A.C_psi*A.S_phi - A.U1*A.phi_dot^2*A.S_psi*A.C_phi)/(A.U1*A.C_psi*A.C_phi); 8 | else 9 | A.U2 = 0; 10 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/Z_meas.m: -------------------------------------------------------------------------------- 1 | function Z_meas 2 | global A 3 | 4 | A.Z_meas = A.Z+A.Z_error(A.counter); %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/copter_model.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | x1=-.27; 4 | x2=.27; 5 | y1=-.01; 6 | y2=.01; 7 | z1=-.01; 8 | z2=.01; 9 | 10 | acon=[x1 y1 z1; 11 | x2 y1 z1; 12 | x2 y2 z1; 13 | x1 y2 z1; 14 | x1 y1 z2; 15 | x2 y1 z2; 16 | x2 y2 z2; 17 | x1 y2 z2]; 18 | 19 | bcon=[1 2 6 5; 20 | 2 3 7 6; 21 | 3 4 8 7; 22 | 4 1 5 8; 23 | 1 2 3 4; 24 | 5 6 7 8]; 25 | 26 | 27 | body1=patch('faces',bcon,... 28 | 'vertices',acon,... 29 | 'facecolor',[.8 .8 .8],... 30 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 31 | 32 | y1=-.27; 33 | y2=.27; 34 | x1=-.01; 35 | x2=.01; 36 | z1=-.01; 37 | z2=.01; 38 | 39 | acon=[x1 y1 z1; 40 | x2 y1 z1; 41 | x2 y2 z1; 42 | x1 y2 z1; 43 | x1 y1 z2; 44 | x2 y1 z2; 45 | x2 y2 z2; 46 | x1 y2 z2]; 47 | 48 | body2=patch('faces',bcon,... 49 | 'vertices',acon,... 50 | 'facecolor',[.8 .8 .8],... 51 | 'edgecolor',[0 0 0],'facecolor',[.4 .5 1]); 52 | 53 | t=0:pi/10:2*pi; 54 | X=.13*cos(t); 55 | Y=.13*sin(t); 56 | Z=zeros(size(t))+.015; 57 | C=zeros(size(t)); 58 | A.C = C; 59 | 60 | RotorF = patch(X+.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 61 | RotorB = patch(X-.27,Y,Z,C,'facealpha',.3,'facecolor','k'); 62 | RotorL = patch(X,Y+.27,Z,C,'facealpha',.3,'facecolor','k'); 63 | RotorR = patch(X,Y-.27,Z,C,'facealpha',.3,'facecolor','k'); 64 | 65 | A.Body1X = get(body1,'xdata'); 66 | A.Body1Y = get(body1,'ydata'); 67 | A.Body1Z = get(body1,'zdata'); 68 | 69 | A.Body2X = get(body2,'xdata'); 70 | A.Body2Y = get(body2,'ydata'); 71 | A.Body2Z = get(body2,'zdata'); 72 | 73 | A.RotorFX = get(RotorF,'xdata'); 74 | A.RotorFY = get(RotorF,'ydata'); 75 | A.RotorFZ = get(RotorF,'zdata'); 76 | 77 | A.RotorBX = get(RotorB,'xdata'); 78 | A.RotorBY = get(RotorB,'ydata'); 79 | A.RotorBZ = get(RotorB,'zdata'); 80 | 81 | A.RotorRX = get(RotorR,'xdata'); 82 | A.RotorRY = get(RotorR,'ydata'); 83 | A.RotorRZ = get(RotorR,'zdata'); 84 | 85 | A.RotorLX = get(RotorL,'xdata'); 86 | A.RotorLY = get(RotorL,'ydata'); 87 | A.RotorLZ = get(RotorL,'zdata'); 88 | 89 | % xlabel('x') 90 | % grid on 91 | % axis([-1 1 -1 1 -1 1]); 92 | axis equal 93 | 94 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/copter_model_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | load Quadrotor 4 | 5 | A.Body1 = patch('xdata',A.Body1X,'ydata',A.Body1Y,'zdata',A.Body1Z,'facealpha',.3,'facecolor','b'); 6 | A.Body2 = patch('xdata',A.Body2X,'ydata',A.Body2Y,'zdata',A.Body2Z,'facealpha',.3,'facecolor','b'); 7 | A.RotorF = patch('xdata',A.RotorFX,'ydata',A.RotorFY,'zdata',A.RotorFZ,'facealpha',.3,'facecolor','k'); 8 | A.RotorB = patch('xdata',A.RotorBX,'ydata',A.RotorBY,'zdata',A.RotorBZ,'facealpha',.3,'facecolor','k'); 9 | A.RotorR = patch('xdata',A.RotorRX,'ydata',A.RotorRY,'zdata',A.RotorRZ,'facealpha',.3,'facecolor','k'); 10 | A.RotorL = patch('xdata',A.RotorLX,'ydata',A.RotorLY,'zdata',A.RotorLZ,'facealpha',.3,'facecolor','k'); 11 | 12 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/example.m: -------------------------------------------------------------------------------- 1 | load wind 2 | wind_speed = sqrt(u.^2 + v.^2 + w.^2); 3 | 4 | % hpatch = patch(isosurface(x,y,z,wind_speed,35)); 5 | % isonormals(x,y,z,wind_speed,hpatch) 6 | % set(hpatch,'FaceColor','red','EdgeColor','none'); 7 | % 8 | % [f vt] = reducepatch(isosurface(x,y,z,wind_speed,45),0.05); 9 | % daspect([1,1,1]); 10 | % hcone = coneplot(x,y,z,u,v,w,vt(:,1),vt(:,2),vt(:,3),2); 11 | % set(hcone,'FaceColor','blue','EdgeColor','none'); 12 | 13 | camproj perspective 14 | camva(25) 15 | 16 | hlight = camlight('headlight'); 17 | % set(hpatch,'AmbientStrength',.1,... 18 | % 'SpecularStrength',1,... 19 | % 'DiffuseStrength',1); 20 | % set(hcone,'SpecularStrength',1); 21 | % set(gcf,'Color','k') 22 | 23 | lighting gouraud 24 | set(gcf,'Renderer','OpenGL') 25 | 26 | hsline = streamline(x,y,z,u,v,w,80,30,11); 27 | xd = get(hsline,'XData'); 28 | yd = get(hsline,'YData'); 29 | zd = get(hsline,'ZData'); 30 | delete(hsline) 31 | tic 32 | for i=1:length(xd)-50 33 | campos([xd(i),yd(i),zd(i)]) 34 | camtarget([xd(i+5)+min(xd)/100,yd(i),zd(i)]) 35 | camlight(hlight,'headlight') 36 | drawnow 37 | toc 38 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/motor.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/motor.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/motor2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/motor2.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/obstacle.m: -------------------------------------------------------------------------------- 1 | function [B,C,D] = obstacle(X,Y,Z) 2 | % B1 = [X1,Y1,Z1] 3 | % C1 = [X2,Y2,Z1] 4 | % D1 = [(X1+X2)/2,(Y1+Y2)/2,Z2] 5 | 6 | B = [X(1),Y(1),Z(1)]; 7 | C = [X(2),Y(2),Z(1)]; 8 | D = [(X(1)+X(2))/2,(Y(1)+Y(2))/2,Z(2)]; 9 | 10 | 11 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/obstacles.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/obstacles.png -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/path.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/path.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/phi1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/phi1.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/phi_meas.m: -------------------------------------------------------------------------------- 1 | function IMU_meas 2 | global A 3 | 4 | A.phi_meas = A.phi+A.phi_error(A.counter) %erros is +- 2 cm 5 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_X.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | axes('fontsize',16) 5 | % cla 6 | hold on 7 | % plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter)+A.X_error(1:A.counter),'y') 8 | plot(A.t_plot(1:A.counter-1),A.X_ref_plot(1:A.counter-1),'b','linewidth',2) 9 | plot(A.t_plot(1:A.counter-1),A.X_plot(1:A.counter-1),'r','linewidth',2) 10 | % plot(A.t_plot(1:A.counter-1),A.X_ref_plot(1:A.counter-1),'b','linewidth',2) 11 | % plot(A.t_plot(1:A.counter),A.X_dis_plot(1:A.counter),'g') 12 | % plot(A.t_plot(1:A.counter),A.X_kalman_plot(1:A.counter),'color',[1 .4 .8]) 13 | a=legend('actual response','set value') 14 | set(a,'fontsize',16) 15 | 16 | xlabel('Time (s)','fontsize',16) 17 | ylabel('X axis (meter)','fontsize',16) 18 | title('X axis vs. time','fontsize',16) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_XY.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % cla 5 | axes('fontsize',16) 6 | hold on 7 | 8 | plot3(A.X_plot(1:A.counter-1),A.Y_plot(1:A.counter-1),A.Z_plot(1:A.counter-1),'linewidth',2) 9 | plot3(A.X_path,A.Y_path,A.Z_path,'+r','linewidth',3) 10 | view(70,40) 11 | grid on 12 | axis equal 13 | axis([-4 2.5 -1 3 0 2]) 14 | % plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter),'r','linewidth',1) 15 | % plot(A.t_plot(1:A.counter),A.Y_ref_plot(1:A.counter),'b') 16 | % plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 17 | 18 | for i=1:A.num_obstacles(1) 19 | patch([A.B1(i,1) A.C1(i,1) A.C1(i,1) A.B1(i,1)],[A.B1(i,2) A.C1(i,2) A.C1(i,2) A.B1(i,2)],[A.B1(i,3) A.B1(i,3) A.D1(i,3) A.D1(i,3)],[0 .4 1]) 20 | end 21 | 22 | a=legend('Quadrotor path','Desired path') 23 | set(a,'fontsize',16) 24 | 25 | xlabel('X axis (m)','fontsize',16) 26 | ylabel('Y axis (m)','fontsize',16) 27 | zlabel('Z axis (m)','fontsize',16) 28 | 29 | title('path traveled','fontsize',16) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_Y.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | axes('fontsize',16) 5 | % cla 6 | hold on 7 | % plot(A.t_plot(1:A.counter),A.Y_plot(1:A.counter)+A.Y_error(1:A.counter),'y') 8 | plot(A.t_plot(1:A.counter-1),A.Y_ref_plot(1:A.counter-1),'b','linewidth',2) 9 | plot(A.t_plot(1:A.counter-1),A.Y_plot(1:A.counter-1),'r','linewidth',2) 10 | % plot(A.t_plot(1:A.counter),A.Y_dis_plot(1:A.counter),'g') 11 | % plot(A.t_plot(1:A.counter),A.Y_kalman_plot(1:A.counter),'color',[1 .4 .8]) 12 | a=legend('actual response','set value') 13 | set(a,'fontsize',16) 14 | 15 | xlabel('time (s)','fontsize',16) 16 | ylabel('Y axis (m)','fontsize',16) 17 | title('Y axis vs. time','fontsize',16) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_Z.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % % cla 5 | % hold on 6 | % plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter)+A.Z_error(1:A.counter),'y') 7 | % plot(A.t_plot(1:A.counter),A.Z_plot(1:A.counter),'r','linewidth',1) 8 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 9 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 10 | % plot(A.t_plot(1:A.counter),A.Z_kalman_plot(1:A.counter),'color',[1 .4 .8]) 11 | % legend('measured response','actual response','set value','disturbances','kalman filter') 12 | % 13 | % xlabel('time (s)') 14 | % ylabel('altitude (m)') 15 | % title('altitude vs. time') 16 | 17 | axes('fontsize',14) 18 | % cla 19 | hold on 20 | % plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter)+A.X_error(1:A.counter),'y') 21 | plot(A.t_plot(1:A.counter-1),A.Z_plot(1:A.counter-1),'r','linewidth',1) 22 | plot(A.t_plot(1:A.counter-1),A.Z_ref_plot(1:A.counter-1),'b') 23 | % plot(A.t_plot(1:A.counter),A.X_dis_plot(1:A.counter),'g') 24 | % plot(A.t_plot(1:A.counter),A.X_kalman_plot(1:A.counter),'color',[1 .4 .8]) 25 | a=legend('actual response','set value') 26 | set(a,'fontsize',14) 27 | 28 | xlabel('Time (s)','fontsize',14) 29 | ylabel('Z axis (meter)','fontsize',14) 30 | title('Z axis vs. time','fontsize',14) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_forces.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | plot(A.t_plot(1:A.counter-1),A.U1_plot(1:A.counter-1),'r') 6 | plot(A.t_plot(1:A.counter-1),A.U2_plot(1:A.counter-1),'g') 7 | plot(A.t_plot(1:A.counter-1),A.U3_plot(1:A.counter-1),'b') 8 | plot(A.t_plot(1:A.counter-1),A.U4_plot(1:A.counter-1),'k') 9 | % plot(A.t_plot(1:A.counter),A.Z_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.Z_dis_plot(1:A.counter),'g') 11 | legend('Throttle','Roll','Pitch','Yaw') 12 | xlabel('time (s)') 13 | ylabel('value') 14 | title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_motors.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | cla 4 | hold on 5 | subplot(2,2,1) 6 | plot(A.t_plot(1:A.counter),A.O1_plot(1:A.counter),'r') 7 | a=legend('front') 8 | set(a,'fontsize',14) 9 | xlabel('time (s)','fontsize',14) 10 | ylabel('motor speed (rpm)','fontsize',14) 11 | 12 | subplot(2,2,2) 13 | plot(A.t_plot(1:A.counter),A.O2_plot(1:A.counter),'g') 14 | a=legend('right') 15 | set(a,'fontsize',14) 16 | xlabel('time (s)','fontsize',14) 17 | ylabel('motor speed (rpm)','fontsize',14) 18 | 19 | subplot(2,2,3) 20 | plot(A.t_plot(1:A.counter),A.O3_plot(1:A.counter),'b') 21 | a=legend('rear') 22 | set(a,'fontsize',14) 23 | xlabel('time (s)','fontsize',14) 24 | ylabel('motor speed (rpm)','fontsize',14) 25 | 26 | subplot(2,2,4) 27 | plot(A.t_plot(1:A.counter),A.O4_plot(1:A.counter),'k') 28 | a=legend('left') 29 | set(a,'fontsize',14) 30 | xlabel('time (s)','fontsize',14) 31 | ylabel('motor speed (rpm)','fontsize',14) 32 | 33 | % title('Forces and Torques vs. time') -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_phi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | axes('fontsize',14) 5 | 6 | hold on 7 | % plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter)+A.phi_error(1:A.counter),'y') 8 | plot(A.t_plot(1:A.counter),A.phi_plot(1:A.counter),'r') 9 | plot(A.t_plot(1:A.counter),A.phi_ref_plot(1:A.counter),'b') 10 | % plot(A.t_plot(1:A.counter),A.phi_dis_plot(1:A.counter),'g') 11 | % plot(A.t_plot(1:A.counter),A.phi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 12 | % legend('measured response','actual response','set value','disturbances','kalman filter') 13 | 14 | % cla 15 | hold on 16 | % plot(A.t_plot(1:A.counter),A.X_plot(1:A.counter)+A.X_error(1:A.counter),'y') 17 | % plot(A.t_plot(1:A.counter-1),A.X_plot(1:A.counter-1),'r','linewidth',1) 18 | % plot(A.t_plot(1:A.counter-1),A.X_ref_plot(1:A.counter-1),'b') 19 | % plot(A.t_plot(1:A.counter),A.X_dis_plot(1:A.counter),'g') 20 | % plot(A.t_plot(1:A.counter),A.X_kalman_plot(1:A.counter),'color',[1 .4 .8]) 21 | a=legend('actual response','set value') 22 | set(a,'fontsize',14) 23 | 24 | xlabel('Time (s)','fontsize',14) 25 | ylabel('phi (rad)','fontsize',14) 26 | title('phi vs. time','fontsize',14) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_psi.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | % hold on 5 | % plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter)+A.psi_error(1:A.counter),'y') 6 | % plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter),'r') 7 | % plot(A.t_plot(1:A.counter),A.psi_ref_plot(1:A.counter),'b') 8 | % plot(A.t_plot(1:A.counter),A.psi_dis_plot(1:A.counter),'g') 9 | % plot(A.t_plot(1:A.counter),A.psi_kalman_plot(1:A.counter),'color',[1 .4 .8]) 10 | % legend('measured response','actual response','set value','disturbances','kalman filter') 11 | % xlabel('time (s)') 12 | % ylabel('psi (rad)') 13 | % title('psi vs. time') 14 | axes('fontsize',14) 15 | 16 | hold on 17 | plot(A.t_plot(1:A.counter),A.psi_plot(1:A.counter),'r') 18 | plot(A.t_plot(1:A.counter),A.psi_ref_plot(1:A.counter),'b') 19 | 20 | a=legend('actual response','set value') 21 | set(a,'fontsize',14) 22 | 23 | xlabel('Time (s)','fontsize',14) 24 | ylabel('psi (rad)','fontsize',14) 25 | title('psi vs. time','fontsize',14) -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_quad.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X2,A.Y2,A.Z2,A.phi,A.theta,A.psi); 4 | set(A.L2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.FPX,A.FPY,A.FPZ,A.phi,A.theta,A.psi); 7 | set(A.L3,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.RPX,A.RPY,A.RPZ,A.phi,A.theta,A.psi); 10 | set(A.L4,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.rPX,A.rPY,A.rPZ,A.phi,A.theta,A.psi); 13 | set(A.L5,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.LPX,A.LPY,A.LPZ,A.phi,A.theta,A.psi); 16 | set(A.L6,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ(A.X1,A.Y1,A.Z1,A.phi,A.theta,A.psi); 19 | set(A.L1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 20 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_quad_3D.m: -------------------------------------------------------------------------------- 1 | global A 2 | 3 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body1X,A.Body1Y,A.Body1Z,A.phi,A.theta,A.psi); 4 | set(A.Body1,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 5 | 6 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.Body2X,A.Body2Y,A.Body2Z,A.phi,A.theta,A.psi); 7 | set(A.Body2,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 8 | 9 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorFX,A.RotorFY,A.RotorFZ,A.phi,A.theta,A.psi); 10 | set(A.RotorF,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 11 | 12 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorBX,A.RotorBY,A.RotorBZ,A.phi,A.theta,A.psi); 13 | set(A.RotorB,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 14 | 15 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorLX,A.RotorLY,A.RotorLZ,A.phi,A.theta,A.psi); 16 | set(A.RotorL,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); 17 | 18 | [A.X22,A.Y22,A.Z22]=rotateXYZ2(A.RotorRX,A.RotorRY,A.RotorRZ,A.phi,A.theta,A.psi); 19 | set(A.RotorR,'xdata',A.X22+A.X,'ydata',A.Y22+A.Y,'zdata',A.Z22+A.Z); -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/plot_theta.m: -------------------------------------------------------------------------------- 1 | % this function will plot the Z value with the desired one. 2 | global A 3 | figure 4 | axes('fontsize',14) 5 | 6 | hold on 7 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter)+A.theta_error(1:A.counter),'color',[.8 .8 .8]) 8 | plot(A.t_plot(1:A.counter),A.theta_plot(1:A.counter),'r') 9 | plot(A.t_plot(1:A.counter),A.theta_ref_plot(1:A.counter),'b') 10 | plot(A.t_plot(1:A.counter),A.theta_dis_plot(1:A.counter),'g') 11 | plot(A.t_plot(1:A.counter),A.theta_kalman_plot(1:A.counter),'color',[0 1 0]) 12 | a=legend('actual response','set value') 13 | set(a,'fontsize',14) 14 | 15 | xlabel('Time (s)','fontsize',14) 16 | ylabel('theta (rad)','fontsize',14) 17 | title('theta vs. time','fontsize',14) 18 | 19 | 20 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/psi1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/psi1.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/quadmodel.m: -------------------------------------------------------------------------------- 1 | function quadmodel 2 | global A 3 | % this function simulate the dynamics of the quadrotor, it will take the 4 | % Euler angels and forces as inputs and give the linear accelerations WRT 5 | % E-Frame and rotational accelerations WRT B-Frame. 6 | 7 | A.X_ddot = (sin(A.psi)*sin(A.phi) + cos(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 8 | A.Y_ddot = (-cos(A.psi)*sin(A.phi) + sin(A.psi)*sin(A.theta)*cos(A.phi))*A.U1/A.m; 9 | A.Z_ddot = -A.g + (cos(A.theta)*cos(A.phi))*A.U1/A.m; 10 | 11 | A.p_dot = ((A.Iyy - A.Izz)*A.q*A.r -A.Jtp*A.q*A.O + A.U2)/A.Ixx; 12 | A.q_dot = ((A.Izz - A.Ixx)*A.p*A.r +A.Jtp*A.p*A.O + A.U3)/A.Iyy; 13 | A.r_dot = ((A.Ixx - A.Iyy)*A.p*A.q + A.U4)/A.Izz; 14 | 15 | A.phi_dot = A.p + sin(A.phi)*tan(A.theta)*A.q + cos(A.phi)*tan(A.theta)*A.r; 16 | A.theta_dot = cos(A.phi)*A.q - sin(A.phi)*A.r; 17 | A.psi_dot = sin(A.phi)/cos(A.theta)*A.q + cos(A.phi)/cos(A.theta)*A.r; 18 | 19 | % Air friction model 20 | A.X_ddot = A.X_ddot - A.X_dot*1.2; 21 | A.Y_ddot = A.Y_ddot - A.Y_dot*1.2; 22 | 23 | % Disturbance model 24 | A.Z_ddot = A.Z_ddot + A.Z_dis/A.m; 25 | 26 | A.psi_dot = A.psi_dot + A.psi_dis/A.Izz*A.Ts; 27 | 28 | A.theta_dot = A.theta_dot + A.theta_dis/A.Iyy*A.Ts; 29 | 30 | A.phi_dot = A.phi_dot + A.phi_dis/A.Ixx*A.Ts; 31 | 32 | 33 | % Calculating the Z velocity & position 34 | A.Z_dot = A.Z_ddot*A.Ts + A.Z_dot; 35 | A.Z = A.Z_dot*A.Ts + A.Z; 36 | 37 | % Calculating the X velocity & position 38 | A.X_dot = A.X_ddot*A.Ts + A.X_dot; 39 | A.X = A.X_dot*A.Ts + A.X; 40 | 41 | % Calculating the Y velocity & position 42 | A.Y_dot = A.Y_ddot*A.Ts + A.Y_dot; 43 | A.Y = A.Y_dot*A.Ts + A.Y; 44 | 45 | % Calculating p,q,r 46 | A.p = A.p_dot*A.Ts+A.p; 47 | A.q = A.q_dot*A.Ts+A.q; 48 | A.r = A.r_dot*A.Ts+A.r; 49 | 50 | % Calculating phi,theta,psi 51 | A.phi = A.phi_dot*A.Ts+A.phi; 52 | A.theta = A.theta_dot*A.Ts+A.theta; 53 | A.psi = A.psi_dot*A.Ts+A.psi; 54 | 55 | % Plotting variables 56 | A.Z_plot(A.counter) = A.Z; 57 | A.Z_ref_plot(A.counter) = A.Z_des; 58 | A.Z_dis_plot(A.counter) = A.Z_dis; 59 | 60 | A.X_plot(A.counter) = A.X; 61 | A.X_ref_plot(A.counter) = A.X_des; 62 | A.X_dis_plot(A.counter) = A.X_dis; 63 | 64 | A.Y_plot(A.counter) = A.Y; 65 | A.Y_ref_plot(A.counter) = A.Y_des; 66 | A.Y_dis_plot(A.counter) = A.Y_dis; 67 | 68 | A.phi_plot(A.counter) = A.phi; 69 | A.phi_ref_plot(A.counter) = A.phi_des; 70 | A.phi_dis_plot(A.counter) = A.phi_dis; 71 | 72 | A.theta_plot(A.counter) = A.theta; 73 | A.theta_ref_plot(A.counter) = A.theta_des; 74 | A.theta_dis_plot(A.counter) = A.theta_dis; 75 | 76 | A.psi_plot(A.counter) = A.psi; 77 | A.psi_ref_plot(A.counter) = A.psi_des; 78 | A.psi_dis_plot(A.counter) = A.psi_dis; 79 | 80 | A.counter = A.counter + 1; 81 | 82 | % Disturbance removal 83 | if(A.Z_dis ~= 0) 84 | A.Z_dis = 0; 85 | end 86 | 87 | if(A.psi_dis ~= 0) 88 | A.psi_dis = 0; 89 | end 90 | 91 | if(A.theta_dis ~= 0) 92 | A.theta_dis = 0; 93 | end 94 | 95 | if(A.phi_dis ~= 0) 96 | A.phi_dis = 0; 97 | end 98 | 99 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/rotateXYZ.m: -------------------------------------------------------------------------------- 1 | function [X,Y,Z]=rotateXYZ(X,Y,Z,phi,theta,psi) 2 | % define rotation matrix 3 | R_roll = [... 4 | 1, 0, 0;... 5 | 0, cos(phi), -sin(phi);... 6 | 0, sin(phi), cos(phi)]; 7 | R_pitch = [... 8 | cos(theta), 0, sin(theta);... 9 | 0, 1, 0;... 10 | -sin(theta), 0, cos(theta)]; 11 | R_yaw = [... 12 | cos(psi), -sin(psi), 0;... 13 | sin(psi), cos(psi), 0;... 14 | 0, 0, 1]; 15 | R = R_roll'*R_pitch'*R_yaw'; 16 | 17 | % rotate vertices 18 | for i=1:length(X) 19 | pts = [X(i), Y(i), Z(i)]*R; 20 | 21 | X(i) = pts(:,1); 22 | Y(i) = pts(:,2); 23 | Z(i) = pts(:,3); 24 | end 25 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/script2run2.m: -------------------------------------------------------------------------------- 1 | 2 | clear all 3 | clc 4 | global A 5 | All_Variables 6 | SimInitial 7 | simLIDARinitial 8 | view(30,30) 9 | grid on 10 | A.Z = .5; 11 | 12 | % for i=1:360 13 | % A.psi = A.psi +1*pi/180; 14 | % Sim_LIDAR 15 | % LIDAR_Plot 16 | % drawnow 17 | % toc 18 | % end 19 | 20 | for i=1:360 21 | A.phi = A.phi +1*pi/180; 22 | Sim_LIDAR 23 | LIDAR_Plot 24 | drawnow 25 | toc 26 | end 27 | 28 | for i=1:360 29 | A.theta = A.theta +1*pi/180; 30 | Sim_LIDAR 31 | LIDAR_Plot 32 | drawnow 33 | toc 34 | end -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/simLIDARinitial.m: -------------------------------------------------------------------------------- 1 | function simLIDARinitial 2 | global A 3 | 4 | % A.LIDARResolution=1; 5 | % A.LIDARMaxRange=4; 6 | % A.LIDARNumberOfBeams=1+180/A.LIDARResolution; 7 | % A.LIDARBeamAngles = (0:A.LIDARResolution*pi/180:pi)'; 8 | % A.LIDARBeamSlopes = tan(A.LIDARBeamAngles); 9 | 10 | A.LIDAR_X_offset = 0; 11 | A.LIDAR_Y_offset = 0; 12 | A.LIDAR_Z_offset = 0; 13 | 14 | 15 | A.LIDAR_X = [A.X1 cos(A.theta11)] + A.LIDAR_X_offset; 16 | A.LIDAR_Y = [A.Y1 sin(A.theta11)] + A.LIDAR_Y_offset; 17 | A.LIDAR_Z = [A.Z1 zeros(1,181)] + A.LIDAR_Z_offset; 18 | A.Cdata = ones(1,182); 19 | 20 | A.LIDAR_Patch = patch(A.LIDAR_X,A.LIDAR_Y,A.LIDAR_Z,A.Cdata,'facecolor','r','FaceAlpha',.3); 21 | 22 | 23 | A.LIDARBeams = 4*ones(1,181); 24 | 25 | A.t=0:180; 26 | 27 | A.X2_init = 4*cosd(A.t); 28 | A.Y2_init = 4*sind(A.t); 29 | A.Z2_init = zeros(1,181); -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/binfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/binfo.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/chart1_ZfxxQE31IEuUdTwkikIDi.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/chart1_ZfxxQE31IEuUdTwkikIDi.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/chart2_9mJDJgrA6frDphDvY5VxDD.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/chart2_9mJDJgrA6frDphDvY5VxDD.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/chart3_01Yt7r9mvv6kgKbp23oyGC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/info/chart3_01Yt7r9mvv6kgKbp23oyGC.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.bat: -------------------------------------------------------------------------------- 1 | call "mexopts.bat" 2 | nmake -f Quadrotor_sfun.mak 3 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.exp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.exp -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.h: -------------------------------------------------------------------------------- 1 | #ifndef __Quadrotor_sfun_h__ 2 | #define __Quadrotor_sfun_h__ 3 | 4 | /* Include files */ 5 | #define S_FUNCTION_NAME sf_sfun 6 | #include "sfc_sf.h" 7 | #include "sfc_mex.h" 8 | #include "rtwtypes.h" 9 | #include "sfcdebug.h" 10 | #define rtInf (mxGetInf()) 11 | #define rtMinusInf (-(mxGetInf())) 12 | #define rtNaN (mxGetNaN()) 13 | #define rtIsNaN(X) ((int)mxIsNaN(X)) 14 | #define rtIsInf(X) ((int)mxIsInf(X)) 15 | 16 | /* Type Definitions */ 17 | 18 | /* Named Constants */ 19 | 20 | /* Variable Declarations */ 21 | extern uint32_T _QuadrotorMachineNumber_; 22 | extern real_T _sfTime_; 23 | 24 | /* Variable Definitions */ 25 | 26 | /* Function Declarations */ 27 | extern void Quadrotor_initializer(void); 28 | extern void Quadrotor_terminator(void); 29 | 30 | /* Function Definitions */ 31 | 32 | /* We load infoStruct for rtw_optimation_info on demand in mdlSetWorkWidths and 33 | free it immediately in mdlStart. Given that this is machine-wide as 34 | opposed to chart specific, we use NULL check to make sure it gets loaded 35 | and unloaded once per machine even though the methods mdlSetWorkWidths/mdlStart 36 | are chart/instance specific. The following methods abstract this out. */ 37 | extern mxArray* load_Quadrotor_optimization_info(void); 38 | extern void unload_Quadrotor_optimization_info(void); 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.lib -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.mak: -------------------------------------------------------------------------------- 1 | # ------------------- Required for MSVC nmake --------------------------------- 2 | # This file should be included at the top of a MAKEFILE as follows: 3 | 4 | 5 | CPU = AMD64 6 | !include 7 | 8 | MACHINE = Quadrotor 9 | TARGET = sfun 10 | CHART_SRCS = \ 11 | c1_Quadrotor.c\ 12 | c2_Quadrotor.c\ 13 | c3_Quadrotor.c 14 | MACHINE_SRC = Quadrotor_sfun.c 15 | MACHINE_REG = Quadrotor_sfun_registry.c 16 | MEX_WRAPPER = 17 | MAKEFILE = Quadrotor_sfun.mak 18 | MATLAB_ROOT = C:\Program Files\MATLAB\R2012a 19 | BUILDARGS = 20 | 21 | #--------------------------- Tool Specifications ------------------------------ 22 | # 23 | # 24 | MSVC_ROOT1 = $(MSDEVDIR:SharedIDE=vc) 25 | MSVC_ROOT2 = $(MSVC_ROOT1:SHAREDIDE=vc) 26 | MSVC_ROOT = $(MSVC_ROOT2:sharedide=vc) 27 | 28 | # Compiler tool locations, CC, LD, LIBCMD: 29 | CC = cl.exe 30 | LD = link.exe 31 | LIBCMD = lib.exe 32 | #------------------------------ Include/Lib Path ------------------------------ 33 | 34 | USER_INCLUDES = 35 | AUX_INCLUDES = 36 | ML_INCLUDES = /I "$(MATLAB_ROOT)\extern\include" 37 | SL_INCLUDES = /I "$(MATLAB_ROOT)\simulink\include" 38 | SF_INCLUDES = /I "C:\Program Files\MATLAB\R2012a\stateflow\c\mex\include" /I "C:\Program Files\MATLAB\R2012a\stateflow\c\debugger\include" 39 | 40 | DSP_INCLUDES = 41 | 42 | COMPILER_INCLUDES = /I "$(MSVC_ROOT)\include" 43 | 44 | INCLUDE_PATH = $(USER_INCLUDES) $(AUX_INCLUDES) $(ML_INCLUDES) $(SL_INCLUDES) $(SF_INCLUDES) $(DSP_INCLUDES) 45 | LIB_PATH = "$(MSVC_ROOT)\lib" 46 | 47 | CFLAGS = /c /Zp8 /GR /W3 /EHs /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DMATLAB_MEX_FILE /nologo /MD $(COMPFLAGS) 48 | LDFLAGS = /nologo /dll /OPT:NOREF /export:mexFunction 49 | AUXLDFLAGS = 50 | 51 | #----------------------------- Source Files ----------------------------------- 52 | 53 | REQ_SRCS = $(MACHINE_SRC) $(MACHINE_REG) $(MEX_WRAPPER) $(CHART_SRCS) 54 | 55 | USER_ABS_OBJS = 56 | 57 | AUX_ABS_OBJS = 58 | 59 | REQ_OBJS = $(REQ_SRCS:.cpp=.obj) 60 | REQ_OBJS2 = $(REQ_OBJS:.c=.obj) 61 | OBJS = $(REQ_OBJS2) $(USER_ABS_OBJS) $(AUX_ABS_OBJS) 62 | OBJLIST_FILE = Quadrotor_sfun.mol 63 | SFCLIB = "C:\Program Files\MATLAB\R2012a\stateflow\c\mex\lib\win64\sfc_mex.lib" "C:\Program Files\MATLAB\R2012a\stateflow\c\debugger\lib\win64\sfc_debug.lib" 64 | AUX_LNK_OBJS = 65 | USER_LIBS = 66 | LINK_MACHINE_LIBS = 67 | 68 | DSP_LIBS = 69 | BLAS_LIBS = "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmwblascompat32.lib" 70 | 71 | #--------------------------------- Rules -------------------------------------- 72 | 73 | MEX_FILE_NAME_WO_EXT = $(MACHINE)_$(TARGET) 74 | MEX_FILE_NAME = $(MEX_FILE_NAME_WO_EXT).mexw64 75 | MEX_FILE_CSF = 76 | all : $(MEX_FILE_NAME) $(MEX_FILE_CSF) 77 | 78 | MEXLIB = "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmx.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmex.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmat.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libfixedpoint.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libut.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libmwmathutil.lib" "C:\Program Files\MATLAB\R2012a\extern\lib\win64\microsoft\libemlrt.lib" "C:\Program Files\MATLAB\R2012a\lib\win64\libippmwipt.lib" 79 | 80 | $(MEX_FILE_NAME) : $(MAKEFILE) $(OBJS) $(SFCLIB) $(AUX_LNK_OBJS) $(USER_LIBS) 81 | @echo ### Linking ... 82 | $(LD) $(LDFLAGS) $(AUXLDFLAGS) /OUT:$(MEX_FILE_NAME) /map:"$(MEX_FILE_NAME_WO_EXT).map" $(USER_LIBS) $(SFCLIB) $(AUX_LNK_OBJS) $(MEXLIB) $(LINK_MACHINE_LIBS) $(DSP_LIBS) $(BLAS_LIBS) @$(OBJLIST_FILE) 83 | mt -outputresource:"$(MEX_FILE_NAME);2" -manifest "$(MEX_FILE_NAME).manifest" 84 | @echo ### Created $@ 85 | 86 | .c.obj : 87 | @echo ### Compiling "$<" 88 | $(CC) $(CFLAGS) $(INCLUDE_PATH) "$<" 89 | 90 | .cpp.obj : 91 | @echo ### Compiling "$<" 92 | $(CC) $(CFLAGS) $(INCLUDE_PATH) "$<" 93 | 94 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.mexw64.manifest: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.mol: -------------------------------------------------------------------------------- 1 | c1_Quadrotor.obj 2 | c2_Quadrotor.obj 3 | c3_Quadrotor.obj 4 | Quadrotor_sfun_registry.obj 5 | Quadrotor_sfun.obj 6 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun_registry.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/Quadrotor_sfun_registry.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c1_Quadrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef __c1_Quadrotor_h__ 2 | #define __c1_Quadrotor_h__ 3 | 4 | /* Include files */ 5 | #include "sfc_sf.h" 6 | #include "sfc_mex.h" 7 | #include "rtwtypes.h" 8 | 9 | /* Type Definitions */ 10 | typedef struct { 11 | const char * context; 12 | const char * name; 13 | const char * dominantType; 14 | const char * resolved; 15 | uint32_T fileTimeLo; 16 | uint32_T fileTimeHi; 17 | uint32_T mFileTimeLo; 18 | uint32_T mFileTimeHi; 19 | } c1_ResolvedFunctionInfo; 20 | 21 | typedef struct { 22 | int32_T c1_sfEvent; 23 | boolean_T c1_isStable; 24 | boolean_T c1_doneDoubleBufferReInit; 25 | uint8_T c1_is_active_c1_Quadrotor; 26 | SimStruct *S; 27 | ChartInfoStruct chartInfo; 28 | uint32_T chartNumber; 29 | uint32_T instanceNumber; 30 | real_T c1_Pp; 31 | boolean_T c1_Pp_not_empty; 32 | real_T c1_Q; 33 | boolean_T c1_Q_not_empty; 34 | real_T c1_R; 35 | boolean_T c1_R_not_empty; 36 | real_T c1_xp; 37 | boolean_T c1_xp_not_empty; 38 | real_T c1_A1; 39 | boolean_T c1_A1_not_empty; 40 | real_T c1_H; 41 | boolean_T c1_H_not_empty; 42 | real_T c1_I; 43 | boolean_T c1_I_not_empty; 44 | real_T c1_xp_dot; 45 | boolean_T c1_xp_dot_not_empty; 46 | } SFc1_QuadrotorInstanceStruct; 47 | 48 | /* Named Constants */ 49 | 50 | /* Variable Declarations */ 51 | 52 | /* Variable Definitions */ 53 | 54 | /* Function Declarations */ 55 | extern const mxArray *sf_c1_Quadrotor_get_eml_resolved_functions_info(void); 56 | 57 | /* Function Definitions */ 58 | extern void sf_c1_Quadrotor_get_check_sum(mxArray *plhs[]); 59 | extern void c1_Quadrotor_method_dispatcher(SimStruct *S, int_T method, void 60 | *data); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c1_Quadrotor.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c1_Quadrotor.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c2_Quadrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef __c2_Quadrotor_h__ 2 | #define __c2_Quadrotor_h__ 3 | 4 | /* Include files */ 5 | #include "sfc_sf.h" 6 | #include "sfc_mex.h" 7 | #include "rtwtypes.h" 8 | 9 | /* Type Definitions */ 10 | typedef struct { 11 | const char * context; 12 | const char * name; 13 | const char * dominantType; 14 | const char * resolved; 15 | uint32_T fileTimeLo; 16 | uint32_T fileTimeHi; 17 | uint32_T mFileTimeLo; 18 | uint32_T mFileTimeHi; 19 | } c2_ResolvedFunctionInfo; 20 | 21 | typedef struct { 22 | int32_T c2_sfEvent; 23 | boolean_T c2_isStable; 24 | boolean_T c2_doneDoubleBufferReInit; 25 | uint8_T c2_is_active_c2_Quadrotor; 26 | SimStruct *S; 27 | ChartInfoStruct chartInfo; 28 | uint32_T chartNumber; 29 | uint32_T instanceNumber; 30 | real_T c2_Pp; 31 | boolean_T c2_Pp_not_empty; 32 | real_T c2_Q; 33 | boolean_T c2_Q_not_empty; 34 | real_T c2_R; 35 | boolean_T c2_R_not_empty; 36 | real_T c2_xp; 37 | boolean_T c2_xp_not_empty; 38 | real_T c2_A1; 39 | boolean_T c2_A1_not_empty; 40 | real_T c2_H; 41 | boolean_T c2_H_not_empty; 42 | real_T c2_I; 43 | boolean_T c2_I_not_empty; 44 | real_T c2_xp_dot; 45 | boolean_T c2_xp_dot_not_empty; 46 | } SFc2_QuadrotorInstanceStruct; 47 | 48 | /* Named Constants */ 49 | 50 | /* Variable Declarations */ 51 | 52 | /* Variable Definitions */ 53 | 54 | /* Function Declarations */ 55 | extern const mxArray *sf_c2_Quadrotor_get_eml_resolved_functions_info(void); 56 | 57 | /* Function Definitions */ 58 | extern void sf_c2_Quadrotor_get_check_sum(mxArray *plhs[]); 59 | extern void c2_Quadrotor_method_dispatcher(SimStruct *S, int_T method, void 60 | *data); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c2_Quadrotor.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c2_Quadrotor.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c3_Quadrotor.h: -------------------------------------------------------------------------------- 1 | #ifndef __c3_Quadrotor_h__ 2 | #define __c3_Quadrotor_h__ 3 | 4 | /* Include files */ 5 | #include "sfc_sf.h" 6 | #include "sfc_mex.h" 7 | #include "rtwtypes.h" 8 | 9 | /* Type Definitions */ 10 | typedef struct { 11 | const char * context; 12 | const char * name; 13 | const char * dominantType; 14 | const char * resolved; 15 | uint32_T fileTimeLo; 16 | uint32_T fileTimeHi; 17 | uint32_T mFileTimeLo; 18 | uint32_T mFileTimeHi; 19 | } c3_ResolvedFunctionInfo; 20 | 21 | typedef struct { 22 | int32_T c3_sfEvent; 23 | boolean_T c3_isStable; 24 | boolean_T c3_doneDoubleBufferReInit; 25 | uint8_T c3_is_active_c3_Quadrotor; 26 | SimStruct *S; 27 | ChartInfoStruct chartInfo; 28 | uint32_T chartNumber; 29 | uint32_T instanceNumber; 30 | real_T c3_Pp; 31 | boolean_T c3_Pp_not_empty; 32 | real_T c3_Q; 33 | boolean_T c3_Q_not_empty; 34 | real_T c3_R; 35 | boolean_T c3_R_not_empty; 36 | real_T c3_xp; 37 | boolean_T c3_xp_not_empty; 38 | real_T c3_A1; 39 | boolean_T c3_A1_not_empty; 40 | real_T c3_H; 41 | boolean_T c3_H_not_empty; 42 | real_T c3_I; 43 | boolean_T c3_I_not_empty; 44 | real_T c3_xp_dot; 45 | boolean_T c3_xp_dot_not_empty; 46 | } SFc3_QuadrotorInstanceStruct; 47 | 48 | /* Named Constants */ 49 | 50 | /* Variable Declarations */ 51 | 52 | /* Variable Definitions */ 53 | 54 | /* Function Declarations */ 55 | extern const mxArray *sf_c3_Quadrotor_get_eml_resolved_functions_info(void); 56 | 57 | /* Function Definitions */ 58 | extern void sf_c3_Quadrotor_get_check_sum(mxArray *plhs[]); 59 | extern void c3_Quadrotor_method_dispatcher(SimStruct *S, int_T method, void 60 | *data); 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c3_Quadrotor.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/c3_Quadrotor.obj -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/mexopts.bat: -------------------------------------------------------------------------------- 1 | @echo off 2 | rem MSVC100OPTS.BAT 3 | rem 4 | rem Compile and link options used for building MEX-files 5 | rem using the Microsoft Visual C++ compiler version 10.0 6 | rem 7 | rem $Revision: 1.1.6.3 $ $Date: 2011/03/09 05:35:31 $ 8 | rem Copyright 2007-2009 The MathWorks, Inc. 9 | rem 10 | rem StorageVersion: 1.0 11 | rem C++keyFileName: MSVC100OPTS.BAT 12 | rem C++keyName: Microsoft Visual C++ 2010 13 | rem C++keyManufacturer: Microsoft 14 | rem C++keyVersion: 10.0 15 | rem C++keyLanguage: C++ 16 | rem 17 | rem ******************************************************************** 18 | rem General parameters 19 | rem ******************************************************************** 20 | 21 | set MATLAB=%MATLAB% 22 | set VSINSTALLDIR=c:\Program Files (x86)\Microsoft Visual Studio 10.0 23 | set VCINSTALLDIR=%VSINSTALLDIR%\VC 24 | rem In this case, LINKERDIR is being used to specify the location of the SDK 25 | set LINKERDIR=C:\Program Files (x86)\Microsoft SDKs\Windows\v7.0A\ 26 | set PATH=%VCINSTALLDIR%\bin\amd64;%VCINSTALLDIR%\bin;%VCINSTALLDIR%\VCPackages;%VSINSTALLDIR%\Common7\IDE;%VSINSTALLDIR%\Common7\Tools;%LINKERDIR%\bin\x64;%LINKERDIR%\bin;%MATLAB_BIN%;%PATH% 27 | set INCLUDE=%VCINSTALLDIR%\INCLUDE;%VCINSTALLDIR%\ATLMFC\INCLUDE;%LINKERDIR%\include;%INCLUDE% 28 | set LIB=%VCINSTALLDIR%\LIB\amd64;%VCINSTALLDIR%\ATLMFC\LIB\amd64;%LINKERDIR%\lib\x64;%MATLAB%\extern\lib\win64;%LIB% 29 | set MW_TARGET_ARCH=win64 30 | 31 | rem ******************************************************************** 32 | rem Compiler parameters 33 | rem ******************************************************************** 34 | set COMPILER=cl 35 | set COMPFLAGS=/c /GR /W3 /EHs /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DMATLAB_MEX_FILE /nologo /MD 36 | set OPTIMFLAGS=/O2 /Oy- /DNDEBUG 37 | set DEBUGFLAGS=/Z7 38 | set NAME_OBJECT=/Fo 39 | 40 | rem ******************************************************************** 41 | rem Linker parameters 42 | rem ******************************************************************** 43 | set LIBLOC=%MATLAB%\extern\lib\win64\microsoft 44 | set LINKER=link 45 | set LINKFLAGS=/dll /export:%ENTRYPOINT% /LIBPATH:"%LIBLOC%" libmx.lib libmex.lib libmat.lib /MACHINE:X64 kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /nologo /manifest /incremental:NO /implib:"%LIB_NAME%.x" /MAP:"%OUTDIR%%MEX_NAME%%MEX_EXT%.map" 46 | set LINKOPTIMFLAGS= 47 | set LINKDEBUGFLAGS=/debug /PDB:"%OUTDIR%%MEX_NAME%%MEX_EXT%.pdb" 48 | set LINK_FILE= 49 | set LINK_LIB= 50 | set NAME_OUTPUT=/out:"%OUTDIR%%MEX_NAME%%MEX_EXT%" 51 | set RSP_FILE_INDICATOR=@ 52 | 53 | rem ******************************************************************** 54 | rem Resource compiler parameters 55 | rem ******************************************************************** 56 | set RC_COMPILER=rc /fo "%OUTDIR%mexversion.res" 57 | set RC_LINKER= 58 | 59 | set POSTLINK_CMDS=del "%LIB_NAME%.x" "%LIB_NAME%.exp" 60 | set POSTLINK_CMDS1=mt -outputresource:"%OUTDIR%%MEX_NAME%%MEX_EXT%;2" -manifest "%OUTDIR%%MEX_NAME%%MEX_EXT%.manifest" 61 | set POSTLINK_CMDS2=del "%OUTDIR%%MEX_NAME%%MEX_EXT%.manifest" 62 | set POSTLINK_CMDS3=del "%OUTDIR%%MEX_NAME%%MEX_EXT%.map" 63 | -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/rtwtypeschksum.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/slprj/_sfprj/Quadrotor/_self/sfun/src/rtwtypeschksum.mat -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/theta1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/theta1.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/x.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/x.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/x1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/x1.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/x2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/x2.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/y.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/y.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/y1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/y1.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/y2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/y2.jpg -------------------------------------------------------------------------------- /3D_with_LIDAR/3D+LIDAR+path/z2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/3D_with_LIDAR/3D+LIDAR+path/z2.jpg -------------------------------------------------------------------------------- /Integrated Simulation Platform for Indoor Quadrotor Applications.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/Integrated Simulation Platform for Indoor Quadrotor Applications.pdf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Muhannad 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 | 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Quadrotor_MATLAB_simulation 2 | a 3D simulation of a Quadrotor using MATLAB 3 | 4 | to run any simulation on MATLAB, just run the script2run.m 5 | 6 | For more information, please check the paper. 7 | 8 | cite 9 | Alomari, M., Jaradat, M., and Jarrah, M. 2013. ``Integrated simulation platform for indoor quadrotor applications." Mechatronics and its Applications (ISMA),9th International Symposium on. IEEE. 10 | 11 | 12 | ![marker](https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/master/doc/quadrotor1.png) 13 | 14 | ![marker](https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/master/doc/quadrotor2.png) 15 | -------------------------------------------------------------------------------- /doc/quadrotor1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/doc/quadrotor1.png -------------------------------------------------------------------------------- /doc/quadrotor2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/OMARI1988/Quadrotor_MATLAB_simulation/3cb2ce031f58f8bbc129403c38bc70296b92217f/doc/quadrotor2.png --------------------------------------------------------------------------------