├── X4 LQG Controller ├── 12States │ ├── Ddt.txt │ ├── U_e.txt │ ├── Cdt.txt │ ├── Kidt.txt │ ├── Bdt.txt │ ├── Kdt.txt │ ├── Model Parameters.txt │ ├── Ldt.txt │ ├── Adt.txt │ ├── Quad_Dynamics_V.m │ ├── Quad_Dynamics.m │ └── ControlDesignLQG12States.m ├── 16States │ ├── Ddt.txt │ ├── U_e.txt │ ├── Cdt.txt │ ├── __pycache__ │ │ ├── AStar.cpython-37.pyc │ │ ├── Quad_Dynamics.cpython-37.pyc │ │ └── Linear_Quadratic_Gaussian.cpython-37.pyc │ ├── Mechanics-Modelling-Planning-Control.png │ ├── Mechanics-Modelling-Planning-Control-Results.png │ ├── Kidt.txt │ ├── Model Parameters.txt │ ├── Kdt.txt │ ├── Bdt.txt │ ├── Ldt.txt │ ├── Adt.txt │ ├── Linear_Quadratic_Gaussian.py │ ├── Quad_Dynamics.m │ ├── Quad_Dynamics.py │ ├── Planning&ControlSim.py │ ├── ControlDesignLQG16StatesJacobian.m │ ├── AStar.py │ ├── ControlDesignLQG16States.m │ └── EKF_Test.m ├── __pycache__ │ ├── AStar.cpython-37.pyc │ ├── Quad_Dynamics.cpython-37.pyc │ └── Linear_Quadratic_Gaussian.cpython-37.pyc └── slprj │ ├── _jitprj │ ├── jitEngineAccessInfo.mat │ ├── sxJJsxFQ4SFBkYFL2xkKJAC.l │ └── sxJJsxFQ4SFBkYFL2xkKJAC.mat │ ├── _sfprj │ ├── EMLReport │ │ ├── emlReportAccessInfo.mat │ │ └── sxJJsxFQ4SFBkYFL2xkKJAC.mat │ ├── precompile │ │ ├── autoInferAccessInfo.mat │ │ └── ltSFbBbR7E4uHENZ6Jy8EE.mat │ └── Y6MultirotorLQRSimulation14State │ │ ├── amsi_serial.mat │ │ └── _self │ │ └── sfun │ │ └── info │ │ └── binfo.mat │ └── sim │ └── varcache │ └── Y6MultirotorLQRSimulation14State │ ├── varInfo.mat │ ├── checksumOfCache.mat │ └── tmwinternal │ └── simulink_cache.xml ├── .gitattributes ├── Y6 LQG Controller ├── LQGControlSimulation.png ├── Y6MultirotorLQRSimulation14State.slx ├── Y6MultirotorLQRSimulation14State.slxc ├── slprj │ ├── _jitprj │ │ ├── jitEngineAccessInfo.mat │ │ ├── sxJJsxFQ4SFBkYFL2xkKJAC.l │ │ └── sxJJsxFQ4SFBkYFL2xkKJAC.mat │ ├── _sfprj │ │ ├── EMLReport │ │ │ ├── emlReportAccessInfo.mat │ │ │ └── sxJJsxFQ4SFBkYFL2xkKJAC.mat │ │ ├── precompile │ │ │ ├── autoInferAccessInfo.mat │ │ │ └── ltSFbBbR7E4uHENZ6Jy8EE.mat │ │ └── Y6MultirotorLQRSimulation14State │ │ │ ├── amsi_serial.mat │ │ │ └── _self │ │ │ └── sfun │ │ │ └── info │ │ │ └── binfo.mat │ └── sim │ │ └── varcache │ │ └── Y6MultirotorLQRSimulation14State │ │ ├── varInfo.mat │ │ ├── checksumOfCache.mat │ │ └── tmwinternal │ │ └── simulink_cache.xml ├── Hex_Dynamics.m └── ControlDesignLQG14States.m ├── MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master ├── MPC_MHE_slides.pdf ├── MPC_tracking.pdf ├── Codes_casadi_v3_4_5 │ ├── motivation_example_1.m │ ├── motivation_example_2.m │ ├── motivation_example_3.m │ ├── motivation_example_3.xlsx │ ├── MPC_code │ │ ├── shift.m │ │ ├── Draw_MPC_tracking_v1.m │ │ ├── Draw_MPC_point_stabilization_v1.m │ │ ├── Draw_MPC_PS_Obstacles.m │ │ ├── Sim_1_MPC_Robot_PS_sing_shooting.m │ │ ├── Sim_2_MPC_Robot_PS_mul_shooting.m │ │ ├── Sim_3_MPC_Robot_PS_obs_avoid_mul_sh.m │ │ └── Sim_4_MPC_Robot_tracking_mul_shooting.m │ └── MHE_code │ │ ├── shift.m │ │ ├── Draw_MPC_point_stabilization_v1.m │ │ ├── MHE_Robot_PS_mul_shooting_v1.m │ │ └── MHE_Robot_PS_mul_shooting_v2.m └── ReadMe.txt ├── README.md ├── Y6 MPC Controller ├── predict_mats.m ├── cost_mats.m ├── cost_mats_cl.m ├── predict_mats_cl.m ├── ompc_cost.m ├── constraint_mats.m ├── constraint_mats_cl.m ├── ompc_suboptcost.m ├── findmas.m ├── ompc_constraints.m ├── Hex_Dynamics.m └── ompc_simulate_constraints.m ├── MatLabJacobianLinearisation.txt ├── SLAM └── EKF-SLAM.py └── Planning ├── AStar.py └── RRTStar.py /X4 LQG Controller/12States/Ddt.txt: -------------------------------------------------------------------------------- 1 | 0,0,0,0 2 | 0,0,0,0 3 | 0,0,0,0 4 | 0,0,0,0 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Ddt.txt: -------------------------------------------------------------------------------- 1 | 0,0,0,0 2 | 0,0,0,0 3 | 0,0,0,0 4 | 0,0,0,0 5 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/U_e.txt: -------------------------------------------------------------------------------- 1 | 207.8639221191406 2 | 207.8639221191406 3 | 207.8639221191406 4 | 207.8639221191406 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/U_e.txt: -------------------------------------------------------------------------------- 1 | 166.4686210181872 2 | 166.4686210181872 3 | 166.4686210181872 4 | 166.4686210181872 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Cdt.txt: -------------------------------------------------------------------------------- 1 | 1,0,0,0,0,0,0,0,0,0,0,0 2 | 0,0,1,0,0,0,0,0,0,0,0,0 3 | 0,0,0,0,1,0,0,0,0,0,0,0 4 | 0,0,0,0,0,0,1,0,0,0,0,0 5 | -------------------------------------------------------------------------------- /Y6 LQG Controller/LQGControlSimulation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/LQGControlSimulation.png -------------------------------------------------------------------------------- /X4 LQG Controller/__pycache__/AStar.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/__pycache__/AStar.cpython-37.pyc -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Cdt.txt: -------------------------------------------------------------------------------- 1 | 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 2 | 0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0 3 | 0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0 4 | 0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_jitprj/jitEngineAccessInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_jitprj/jitEngineAccessInfo.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/Y6MultirotorLQRSimulation14State.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/Y6MultirotorLQRSimulation14State.slx -------------------------------------------------------------------------------- /Y6 LQG Controller/Y6MultirotorLQRSimulation14State.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/Y6MultirotorLQRSimulation14State.slxc -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_jitprj/jitEngineAccessInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_jitprj/jitEngineAccessInfo.mat -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.l -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.l -------------------------------------------------------------------------------- /X4 LQG Controller/16States/__pycache__/AStar.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/16States/__pycache__/AStar.cpython-37.pyc -------------------------------------------------------------------------------- /X4 LQG Controller/__pycache__/Quad_Dynamics.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/__pycache__/Quad_Dynamics.cpython-37.pyc -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_jitprj/sxJJsxFQ4SFBkYFL2xkKJAC.mat -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_sfprj/precompile/autoInferAccessInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_sfprj/precompile/autoInferAccessInfo.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_sfprj/EMLReport/emlReportAccessInfo.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_sfprj/precompile/autoInferAccessInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_sfprj/precompile/autoInferAccessInfo.mat -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Mechanics-Modelling-Planning-Control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/16States/Mechanics-Modelling-Planning-Control.png -------------------------------------------------------------------------------- /X4 LQG Controller/16States/__pycache__/Quad_Dynamics.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/16States/__pycache__/Quad_Dynamics.cpython-37.pyc -------------------------------------------------------------------------------- /X4 LQG Controller/__pycache__/Linear_Quadratic_Gaussian.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/__pycache__/Linear_Quadratic_Gaussian.cpython-37.pyc -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_sfprj/EMLReport/sxJJsxFQ4SFBkYFL2xkKJAC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_sfprj/EMLReport/sxJJsxFQ4SFBkYFL2xkKJAC.mat -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_sfprj/precompile/ltSFbBbR7E4uHENZ6Jy8EE.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_sfprj/precompile/ltSFbBbR7E4uHENZ6Jy8EE.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_sfprj/EMLReport/sxJJsxFQ4SFBkYFL2xkKJAC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_sfprj/EMLReport/sxJJsxFQ4SFBkYFL2xkKJAC.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_sfprj/precompile/ltSFbBbR7E4uHENZ6Jy8EE.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_sfprj/precompile/ltSFbBbR7E4uHENZ6Jy8EE.mat -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/MPC_MHE_slides.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/MPC_MHE_slides.pdf -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/MPC_tracking.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/MPC_tracking.pdf -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Mechanics-Modelling-Planning-Control-Results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/16States/Mechanics-Modelling-Planning-Control-Results.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Planning-SLAM-and-Control 2 | Path and Trajectory Planning: (A* RRT), Simultaneous Localisation and Mapping: (EKF,FAST),[Source: Python Robotics] and Control Systems: (LQG,MPC),[Source: UWaterloo Mohamed W. Mehrez] 3 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/__pycache__/Linear_Quadratic_Gaussian.cpython-37.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/16States/__pycache__/Linear_Quadratic_Gaussian.cpython-37.pyc -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/amsi_serial.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/amsi_serial.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/amsi_serial.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/amsi_serial.mat -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/varInfo.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/varInfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/varInfo.mat -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/_self/sfun/info/binfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/_self/sfun/info/binfo.mat -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/X4 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/checksumOfCache.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/_self/sfun/info/binfo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/_sfprj/Y6MultirotorLQRSimulation14State/_self/sfun/info/binfo.mat -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/checksumOfCache.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/Y6 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/checksumOfCache.mat -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_1.m -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_2.m -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_3.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_3.m -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_3.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Planning-SLAM-and-Control/HEAD/MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/motivation_example_3.xlsx -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MPC_code/shift.m: -------------------------------------------------------------------------------- 1 | function [t0, x0, u0] = shift(T, t0, x0, u,f) 2 | st = x0; 3 | con = u(1,:)'; 4 | f_value = f(st,con); 5 | st = st+ (T*f_value); 6 | x0 = full(st); 7 | 8 | t0 = t0 + T; 9 | u0 = [u(2:size(u,1),:);u(size(u,1),:)]; 10 | end -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Kidt.txt: -------------------------------------------------------------------------------- 1 | 119.5469055175781,-187.0571441650391,-187.0571441650391,-33.12178421020508 2 | 119.5469818115234,-187.0571441650391,187.0570983886719,33.12177658081055 3 | 119.5469360351562,187.0571136474609,-187.05712890625,33.12177276611328 4 | 119.5469284057617,187.0571136474609,187.0570983886719,-33.12178039550781 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Kidt.txt: -------------------------------------------------------------------------------- 1 | 88.30256637422313,-88.30256705150971,101.6177288271717,-29.97417321325548 2 | -88.30256731459895,-88.30256093586586,101.6177221780958,29.97417174234594 3 | 88.30256011235929,88.30256414071953,101.6177287486859,29.97417243265819 4 | -88.30255714376359,88.30256454285218,101.6177226282389,-29.9741725502494 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8HLSkgJs4X0mcRvkaNjzbw== 5 | 6 | -------------------------------------------------------------------------------- /Y6 LQG Controller/slprj/sim/varcache/Y6MultirotorLQRSimulation14State/tmwinternal/simulink_cache.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 8HLSkgJs4X0mcRvkaNjzbw== 5 | 6 | -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MHE_code/shift.m: -------------------------------------------------------------------------------- 1 | function [t0, x0, u0] = shift(T, t0, x0, u,f) 2 | % add noise to the control actions before applying it 3 | con_cov = diag([0.005 deg2rad(2)]).^2; 4 | con = u(1,:)' + sqrt(con_cov)*randn(2,1); 5 | st = x0; 6 | 7 | f_value = f(st,con); 8 | st = st+ (T*f_value); 9 | 10 | x0 = full(st); 11 | t0 = t0 + T; 12 | 13 | u0 = [u(2:size(u,1),:);u(size(u,1),:)]; % shift the control action 14 | end -------------------------------------------------------------------------------- /Y6 MPC Controller/predict_mats.m: -------------------------------------------------------------------------------- 1 | function [F,G] = predict_mats(A,B,N) 2 | % 3 | % PREDICT_MATS.M returns the MPC prediction matrices for a system 4 | % 5 | % x^+ = A*x + B*u 6 | % 7 | % That is, the matrices F and G from the equation 8 | % 9 | % X = F*x + G*U 10 | % 11 | % USAGE: 12 | % 13 | % [F,G] = predict_mats(A,B,N) 14 | % 15 | % where N is prediction horizon length. 16 | % 17 | % P. Trodden, 2015. 18 | 19 | % dimensions 20 | n = size(A,1); 21 | m = size(B,2); 22 | 23 | % allocate 24 | F = zeros(n*N,n); 25 | G = zeros(n*N,m*(N-1)); 26 | 27 | % form row by row 28 | for i = 1:N 29 | % F 30 | F(n*(i-1)+(1:n),:) = A^i; 31 | 32 | % G 33 | % form element by element 34 | for j = 1:i 35 | G(n*(i-1)+(1:n),m*(j-1)+(1:m)) = A^(i-j)*B; 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Model Parameters.txt: -------------------------------------------------------------------------------- 1 | X4 Quadcopter v151 2 | 3 | Component Instances (1) 4 | Area 3343.05 cm^2 5 | Density 2.06 g / cm^3 6 | Mass 857.945 g 7 | Volume 416.426 cm^3 8 | Physical Material (Various) 9 | 10 | Bounding Box 11 | Length 64.307 cm 12 | Width 12.873 cm 13 | Height 63.481 cm 14 | World X,Y,Z 0.00 cm, 0.00 cm, 0.00 cm 15 | Center of Mass 0.01 cm, 0.167 cm, -0.029 cm 16 | 17 | Moment of Inertia at Center of Mass (g cm^2) 18 | Ixx = 1.061E+05 19 | Ixy = 23.775 20 | Ixz = 944.869 21 | Iyx = 23.775 22 | Iyy = 2.011E+05 23 | Iyz = -61.225 24 | Izx = 944.869 25 | Izy = -61.225 26 | Izz = 1.061E+05 27 | 28 | Moment of Inertia at Origin (g cm^2) 29 | Ixx = 1.061E+05 30 | Ixy = 22.35 31 | Ixz = 945.114 32 | Iyx = 22.35 33 | Iyy = 2.011E+05 34 | Iyz = -57.134 35 | Izx = 945.114 36 | Izy = -57.134 37 | Izz = 1.062E+05 -------------------------------------------------------------------------------- /Y6 MPC Controller/cost_mats.m: -------------------------------------------------------------------------------- 1 | function [H,L,M] = cost_mats(F,G,Q,R,P) 2 | % 3 | % COST_MATS.M returns the MPC cost matrices for a system 4 | % 5 | % X = F*x + G*U 6 | % 7 | % with finite-horizon objective function 8 | % 9 | % J = 0.5*sum_{j=0}^{N-1} x(j)'*Q*x(j) + u(j)'*R*u(j) + x(N)'*P*x(N) 10 | % 11 | % That is, the matrices H, L and M in 12 | % 13 | % J = 0.5*U'*H*U + x'*L'*U + x'*M*x 14 | % 15 | % USAGE: 16 | % 17 | % [H,L,M] = cost_mats(F,G,Q,R,P) 18 | % 19 | % P. Trodden, 2015. 20 | 21 | % get dimensions 22 | n = size(F,2); 23 | N = size(F,1)/n; 24 | 25 | % diagonalize Q and R 26 | Qd = kron(eye(N-1),Q); 27 | Qd = blkdiag(Qd,P); 28 | Rd = kron(eye(N),R); 29 | 30 | % Hessian 31 | H = 2*G'*Qd*G + 2*Rd; 32 | 33 | % Linear term 34 | L = 2*G'*Qd*F; 35 | 36 | % Constant term 37 | M = F'*Qd*F + Q; 38 | 39 | % make sure the Hessian is symmetric 40 | H=(H+H')/2; 41 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Bdt.txt: -------------------------------------------------------------------------------- 1 | -5.856070828258453e-08,-5.856070828258453e-08,-5.856070828258453e-08,-5.856070828258453e-08 2 | -1.696219078439754e-05,-1.696219078439754e-05,-1.696219078439754e-05,-1.696219078439754e-05 3 | 9.799329063753248e-07,9.799329063753248e-07,-9.799329063753248e-07,-9.799329063753248e-07 4 | 0.0002838389191310853,0.0002838389191310853,-0.0002838389191310853,-0.0002838389191310853 5 | 9.799329063753248e-07,-9.799329063753248e-07,9.799329063753248e-07,-9.799329063753248e-07 6 | 0.0002838389191310853,-0.0002838389191310853,0.0002838389191310853,-0.0002838389191310853 7 | 2.837088253215825e-08,-2.837088253215825e-08,-2.837088253215825e-08,2.837088253215825e-08 8 | 8.217665708798449e-06,-8.217665708798449e-06,-8.217665708798449e-06,8.217665708798449e-06 9 | 4.166187763214111,0,0,0 10 | 0,4.166187763214111,0,0 11 | 0,0,4.166187763214111,0 12 | 0,0,0,4.166187763214111 13 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Kdt.txt: -------------------------------------------------------------------------------- 1 | -5617.91796875,-664.990234375,2718.662353515625,136.333740234375,2718.662353515625,136.333740234375,4793.78564453125,843.30029296875,0.06442636996507645,0.00321574998088181,0.003215757431462407,-0.03348331153392792 2 | -5617.9208984375,-664.990478515625,2718.662353515625,136.3337249755859,-2718.66162109375,-136.3337097167969,-4793.78466796875,-843.2999877929688,0.00321575254201889,0.06442635506391525,-0.03348332270979881,0.003215755801647902 3 | -5617.9189453125,-664.990234375,-2718.661865234375,-136.3337249755859,2718.662109375,136.3337249755859,-4793.78466796875,-843.2999267578125,0.003215758129954338,-0.03348332270979881,0.06442636996507645,0.003215758595615625 4 | -5617.9189453125,-664.9904174804688,-2718.661865234375,-136.3337249755859,-2718.66162109375,-136.3337097167969,4793.78564453125,843.3002319335938,-0.03348330780863762,0.003215755103155971,0.003215759992599487,0.06442637741565704 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Model Parameters.txt: -------------------------------------------------------------------------------- 1 | General 2 | Part Number StackBot Air 3 | Part Name StackBot Air v29 4 | Description 5 | Material Name (Various) 6 | 7 | Manage 8 | Item Number 9 | Lifecycle 10 | Revision 11 | State 12 | Change Order 13 | 14 | Physical 15 | Mass 1157.685 g 16 | Volume 477.37 cm^3 17 | Density 2.425 g / cm^3 18 | Area 3826.941 cm^2 19 | World X,Y,Z 0.00 cm, 0.00 cm, 0.00 cm 20 | Center of Mass 0.00 cm, 1.353 cm, -0.013 cm 21 | Bounding Box 22 | Length 64.307 cm 23 | Width 16.733 cm 24 | Height 63.481 cm 25 | Moment of Inertia at Center of Mass (g cm^2) 26 | Ixx 1.129E+05 27 | Ixy 79.795 28 | Ixz 955.236 29 | Iyx 79.795 30 | Iyy 2.033E+05 31 | Iyz -100.828 32 | Izx 955.236 33 | Izy -100.828 34 | Izz 1.129E+05 35 | Moment of Inertia at Origin (g cm^2) 36 | Ixx 1.150E+05 37 | Ixy 80.071 38 | Ixz 955.233 39 | Iyx 80.071 40 | Iyy 2.033E+05 41 | Iyz -80.30 42 | Izx 955.233 43 | Izy -80.30 44 | Izz 1.150E+05 45 | 46 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Ldt.txt: -------------------------------------------------------------------------------- 1 | 0.9905002117156982,3.399530006985937e-17,-9.140384811635868e-17,-1.10555906600295e-17 2 | 3.082170248031616,2.208402934806251e-15,2.379559663732728e-14,2.343871878675947e-15 3 | 3.399530006985937e-17,0.999005138874054,5.535228478396727e-19,7.0329982096919e-19 4 | 5.790277111950015e-13,0.3159286379814148,-4.606497881710567e-14,-8.650953488720942e-14 5 | -9.140384811635868e-17,5.535228478396727e-19,0.999005138874054,-7.480625447211323e-19 6 | 2.489959058165833e-14,-6.983851702242055e-14,0.3159286379814148,-2.389285872015506e-13 7 | -1.10555906600295e-17,7.0329982096919e-19,-7.480625447211323e-19,0.999005138874054 8 | 6.68126036267036e-14,-1.110228648923926e-13,-5.749933407010743e-14,0.3154135048389435 9 | -0.00141858821734786,0.002595455152913928,0.002595455152913928,7.514435128541663e-05 10 | -0.00141858821734786,0.002595455152913928,-0.002595455152913928,-7.514435128541663e-05 11 | -0.00141858821734786,-0.002595455152913928,0.002595455152913928,-7.514435128541663e-05 12 | -0.00141858821734786,-0.002595455152913928,-0.002595455152913928,7.514435128541663e-05 13 | -------------------------------------------------------------------------------- /Y6 MPC Controller/cost_mats_cl.m: -------------------------------------------------------------------------------- 1 | function [Sc,Scx] = cost_mats_cl(Fxcl,Gxcl,Fycl,Gycl,Fucl,Gucl,Q,R,P) 2 | % 3 | % COST_MATS.M returns the MPC cost matrices for a system 4 | % 5 | % X = F*x + G*U 6 | % 7 | % with finite-horizon objective function 8 | % 9 | % J = 0.5*sum_{j=0}^{N-1} x(j)'*Q*x(j) + u(j)'*R*u(j) + x(N)'*P*x(N) 10 | % 11 | % That is, the matrices H, L and M in 12 | % 13 | % J = 0.5*U'*H*U + x'*L'*U + x'*M*x 14 | % J = = cT Sc c + 2cT Scx x + k 15 | % Sc = HTc diag(Q)Hc + HTcu diag(R) Hcu + HTc2 P Hc2 16 | % Scx = HTc diag(Q)Pcl + HTcu diag(R)Pclu + HTc2 P Pcl2 17 | % 18 | % USAGE: 19 | % 20 | % [H,L,M] = cost_mats(F,G,Q,R,P) 21 | % [H,L,M,Sc,Scx] = cost_mats_cl(Fxcl,Gxcl,Fycl,Gycl,Fucl,Gucl,Q,R,P) 22 | % 23 | % P. Trodden, 2015. 24 | 25 | % get dimensions 26 | n = size(Fxcl,2); 27 | N = size(Fxcl,1)/n; 28 | 29 | % diagonalize Q and R 30 | Qd = kron(eye(N-1),Q); 31 | Qd = blkdiag(Qd,P); 32 | Rd = kron(eye(N),R); 33 | 34 | % Hessian 35 | Sc = 2*Gxcl'*Qd*Gxcl + 2*Gucl'*Rd*Gucl; 36 | 37 | % make sure the Hessian is symmetric 38 | Sc = (Sc+Sc')/2; 39 | 40 | % Linear term 41 | Scx = 2*Gxcl'*Qd*Fxcl + 2*Gucl'*Rd*Fucl; 42 | 43 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Adt.txt: -------------------------------------------------------------------------------- 1 | 1,0.009999999776482582,0,0,0,0,0,0,-3.290434591463054e-08,-3.290434591463054e-08,-3.290434591463054e-08,-3.290434591463054e-08 2 | 0,1,0,0,0,0,0,0,-6.130515885161003e-06,-6.130515885161003e-06,-6.130515885161003e-06,-6.130515885161003e-06 3 | 0,0,1,0.009999999776482582,0,0,0,0,5.506089451046137e-07,5.506089451046137e-07,-5.506089451046137e-07,-5.506089451046137e-07 4 | 0,0,0,1,0,0,0,0,0.0001025857491185889,0.0001025857491185889,-0.0001025857491185889,-0.0001025857491185889 5 | 0,0,0,0,1,0.009999999776482582,0,0,5.506089451046137e-07,-5.506089451046137e-07,5.506089451046137e-07,-5.506089451046137e-07 6 | 0,0,0,0,0,1,0,0,0.0001025857491185889,-0.0001025857491185889,0.0001025857491185889,-0.0001025857491185889 7 | 0,0,0,0,0,0,1,0.009999999776482582,1.594115595082712e-08,-1.594115595082712e-08,-1.594115595082712e-08,1.594115595082712e-08 8 | 0,0,0,0,0,0,0,1,2.97004862659378e-06,-2.97004862659378e-06,-2.97004862659378e-06,2.97004862659378e-06 9 | 0,0,0,0,0,0,0,0,0.6426210999488831,0,0,0 10 | 0,0,0,0,0,0,0,0,0,0.6426210999488831,0,0 11 | 0,0,0,0,0,0,0,0,0,0,0.6426210999488831,0 12 | 0,0,0,0,0,0,0,0,0,0,0,0.6426210999488831 13 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Kdt.txt: -------------------------------------------------------------------------------- 1 | -5931.236678187158,-1580.133503469787,5931.236728198496,1580.133518832059,-4644.134031758053,-576.3632284693124,2234.613786897841,125.0962697581361,2234.613758071289,125.0962661936591,4018.651561917152,840.6199978095586,0.0547414724722801,0.00378154330747522,0.003781541923802913,-0.02695110045085749 2 | 5931.236745030379,1580.133521916061,5931.236319117051,1580.133411221796,-4644.13371535764,-576.3631844021111,2234.613637875068,125.0962620601587,-2234.613782939145,-125.0962670979872,-4018.651360916638,-840.6199522306352,0.003781542802301662,0.05474147759628678,-0.02695109973852811,0.003781543189470837 3 | -5931.236267314834,-1580.1333998624,-5931.236535089917,-1580.133468705661,-4644.134015426424,-576.3632210024908,-2234.613719145838,-125.0962666015587,2234.613631787595,125.0962644005892,-4018.651454526863,-840.6199726752477,0.003781542007346325,-0.02695109967881468,0.05474147557432486,0.003781540562512681 4 | 5931.236064820042,1580.133344255566,-5931.236558235511,-1580.133473413705,-4644.133748507903,-576.3631936860353,-2234.61372256776,-125.0962661429825,-2234.613548512463,-125.0962587433734,4018.651474195543,840.6199803240953,-0.02695110009193574,0.003781542613204318,0.003781540698383591,0.05474147770853746 5 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Bdt.txt: -------------------------------------------------------------------------------- 1 | -4.242674396630788e-11,4.242674396630788e-11,-4.242674396630788e-11,4.242674396630788e-11 2 | -2.091655379100865e-08,2.091655379100865e-08,-2.091655379100865e-08,2.091655379100865e-08 3 | 4.242674396630788e-11,4.242674396630788e-11,-4.242674396630788e-11,-4.242674396630788e-11 4 | 2.091655379100865e-08,2.091655379100865e-08,-2.091655379100865e-08,-2.091655379100865e-08 5 | -6.328350532572434e-08,-6.328350532572434e-08,-6.328350532572434e-08,-6.328350532572434e-08 6 | -1.833015510785475e-05,-1.833015510785475e-05,-1.833015510785475e-05,-1.833015510785475e-05 7 | 8.350800973538338e-07,8.350800973538338e-07,-8.350800973538338e-07,-8.350800973538338e-07 8 | 0.0002418821086662523,0.0002418821086662523,-0.0002418821086662523,-0.0002418821086662523 9 | 8.350800973538338e-07,-8.350800973538338e-07,8.350800973538338e-07,-8.350800973538338e-07 10 | 0.0002418821086662523,-0.0002418821086662523,0.0002418821086662523,-0.0002418821086662523 11 | 2.296949316596679e-08,-2.296949316596679e-08,-2.296949316596679e-08,2.296949316596679e-08 12 | 6.653145560030046e-06,-6.653145560030046e-06,-6.653145560030046e-06,6.653145560030046e-06 13 | 4.166187783441478,0,0,0 14 | 0,4.166187783441478,0,0 15 | 0,0,4.166187783441478,0 16 | 0,0,0,4.166187783441478 17 | -------------------------------------------------------------------------------- /MatLabJacobianLinearisation.txt: -------------------------------------------------------------------------------- 1 | %% Model simplification 2 | % sin x -> x; cos x -> 1; tan x -> x; no distrurbances 3 | syms x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12 4 | syms u1 u2 u3 u4 5 | 6 | xdot1=x4; 7 | xdot2=x5; 8 | xdot3=x6; 9 | xdot4=(u1/m)*(x8+x9*x7)-kx*x4/m; 10 | xdot5=(u1/m)*(x9*x8-x7)-ky*x5/m; 11 | xdot6=(u1/m)-g-kz*x6/m; 12 | xdot7=x10+x11*x7*x8+x12*x8; 13 | xdot8=x11-x12*x7; 14 | xdot9=x7*x11+x12; 15 | xdot10=(u2/Ix)-((Iy-Iz)/Ix)*x11*x12; 16 | xdot11=(u3/Iy)-((Iz-Ix)/Iy)*x10*x12; 17 | xdot12=(u4/Iz)-((Ix-Iy)/Iz)*x10*x11; 18 | 19 | xdot=[xdot1 xdot2 xdot3 xdot4 xdot5 xdot6 xdot7 xdot8 xdot9 xdot10 xdot11 xdot12].'; 20 | x=[x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12].'; 21 | u=[u1 u2 u3 u4].'; 22 | y=[x1 x2 x3 x9].'; 23 | 24 | [x_size,aux]=size(x); 25 | [y_size,aux]=size(y); 26 | [u_size,aux]=size(u); 27 | 28 | %xdot=subs(xdot,[u1 u2 u3 u4],[m*g 0 0 0]) 29 | %[e1 e2 e3 e4 e5 e6 e7 e8 e9 e10 e11 e12]=solve(xdot,[x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12]) 30 | % Equilibrium points 31 | %ue=[m*g 0 0 0].'; 32 | %xe=[x1 x2 x3 0 0 0 0 0 0 0 0 0].'; 33 | 34 | %% Jacobian linearization 35 | % Equilibrium points 36 | ue=[m*g 0 0 0].'; 37 | xe=[x1 x2 x3 0 0 0 0 0 0 0 0 0].'; 38 | 39 | 40 | JA=jacobian(xdot,x.'); 41 | JB=jacobian(xdot,u.'); 42 | JC=jacobian(y, x.'); 43 | 44 | A=subs(JA,[x,u],[xe,ue]); A=eval(A); 45 | B=subs(JB,[x,u],[xe,ue]); B=eval(B); 46 | C=subs(JC,[x,u],[xe,ue]); C=eval(C); 47 | -------------------------------------------------------------------------------- /Y6 MPC Controller/predict_mats_cl.m: -------------------------------------------------------------------------------- 1 | function [Fxcl,Gxcl,Fycl,Gycl,Fucl,Gucl] = predict_mats_cl(Phi,B,C,K,N) 2 | % 3 | % PREDICT_MATS.M returns the MPC prediction matrices for a system 4 | % 5 | % phi = A-BK 6 | % u = -K*x + c 7 | % x^+ = phi*x + B*c 8 | % y^+ = C*(phi*x + B*c) 9 | % u^+ = K*(phi*x + B*c) 10 | % 11 | % That is, the various matrices F and G from the equations 12 | % 13 | % Xcl = Fxcl*x + Gxcl*c 14 | % Ycl = Fycl*x + Gycl*c 15 | % Ucl = Fucl*x + Gucl*c 16 | % 17 | % USAGE: 18 | % 19 | % [Fxcl,Gxcl,Fycl,Gycl,Fucl,Gucl] = predict_mats_cl(phi,B,C,K,N) 20 | % 21 | % where N is prediction horizon length. 22 | % 23 | % P. Trodden, 2015. 24 | 25 | % dimensions 26 | n = size(Phi,1); 27 | m = size(B,2); 28 | p = size(C,1); 29 | 30 | % allocate 31 | Fxcl = zeros(n*N,n); 32 | Fycl = zeros(p*N,n); 33 | Fucl = zeros(m*N,n); 34 | Gxcl = zeros(n*N,m*(N-1)); 35 | Gycl = zeros(p*N,m*(N-1)); 36 | Gucl = zeros(m*N,m*(N-1)); 37 | 38 | 39 | % form row by row 40 | for i = 1:N 41 | % F 42 | Fxcl(n*(i-1)+(1:n),:) = Phi^i; 43 | Fycl(p*(i-1)+(1:p),:) = C * (Phi^i); 44 | Fucl(m*(i-1)+(1:m),:) = -K * (Phi^(i-1)); 45 | 46 | % G 47 | % form element by element 48 | for j = 1:i 49 | Gxcl(n*(i-1)+(1:n),m*(j-1)+(1:m)) = Phi^(i-j)*B; 50 | Gycl(p*(i-1)+(1:p),m*(j-1)+(1:m)) = C * (Phi^(i-j))*B; 51 | Gucl(m*(i-1)+(1:m),m*(j-1)+(1:m)) = K * (Phi^(i-j-1))*B; 52 | end 53 | end 54 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Ldt.txt: -------------------------------------------------------------------------------- 1 | 0.991020653605142,3.837854570286927e-16,-5.777292405109711e-15,4.60702391198652e-18 2 | 8.500679567204088,2.116501878382389e-13,8.647494891743532e-13,-2.042132335058336e-14 3 | 3.837854570286918e-16,0.9910206536051416,-2.005049541470718e-17,2.404433674466714e-18 4 | -7.117252949059306e-14,8.500679567201152,-2.568154016471388e-13,9.409920357285613e-15 5 | -5.777292405109453e-15,-2.005049541470692e-17,0.9905002378172374,-1.976991812363421e-18 6 | 1.264085550233922e-12,1.574386032708646e-14,3.082170426839506,1.60665040207193e-14 7 | -4.117263430928183e-13,3.259378915889479,-1.181738409488683e-12,-8.876177547313974e-15 8 | 6.567948857395017e-13,0.9487274195571145,3.361009128878691e-15,1.302780707441408e-13 9 | -3.259378915891189,-6.232175121574101e-13,-5.680633980402258e-13,-9.161312048021907e-16 10 | -0.9487274195581714,-1.081396228024069e-13,-5.585474935297485e-13,3.138320198180853e-14 11 | 4.607023911985615e-18,2.404433674466112e-18,-1.976991812362887e-18,0.9990051459896656 12 | 1.258677019644958e-13,2.923461110100117e-13,-3.34930859162779e-13,0.3154133480930808 13 | -8.678142340833089e-05,8.678142211025515e-05,-0.001532994372354783,6.08379950903958e-05 14 | 8.678142230761249e-05,8.678141950138432e-05,-0.001532994371125296,-6.083799491243273e-05 15 | -8.678142626407786e-05,-8.678142077776237e-05,-0.001532994371719087,-6.083799503603039e-05 16 | 8.678142270679121e-05,-8.678142315132575e-05,-0.001532994372313401,6.083799499375957e-05 17 | -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/ReadMe.txt: -------------------------------------------------------------------------------- 1 | Copyright © 2017 by Mohamed W. Mehrez 2 | All rights reserved. 3 | 4 | This is a workshop on implementing model predictive control (MPC) and moving horizon estimation (MHE) on Matlab. 5 | The implementation is based on the Casadi Package which is used for numerical optimization. 6 | A non-holonomic mobile robot is used as a system for the implementation. 7 | The workshop video recording can be found here 8 | https://www.youtube.com/playlist?list=PLK8squHT_Uzej3UCUHjtOtm5X7pMFSgAL 9 | 10 | Casadi can be downloaded here https://web.casadi.org/ 11 | 12 | personal website: https://sites.google.com/view/mwmehrez 13 | e-mail: m.mehrez.said@gmail.com 14 | 15 | If you use the codes, please cite any of my related articles. 16 | you can find my articles on google scholar here 17 | https://scholar.google.ca/citations?user=aRNeH4QAAAAJ&hl=en 18 | 19 | This folder contains the presentation material and codes used in 20 | the MPC workshop taken place at the faculty of Engineering, University of Waterloo, on the 24th and the 25th of January, 2019. 21 | 22 | The contents of the folder are: 23 | 1- The presentation slides. 24 | 2- MATLAB codes of all the examples shown. 25 | 26 | Before running the codes, make sure that casadi package is properly installed and added to the matlab path. 27 | you can download casadi from here https://web.casadi.org/get/ 28 | 29 | please message me in case you have any questions 30 | -------------------------------------------------------------------------------- /Y6 MPC Controller/ompc_cost.m: -------------------------------------------------------------------------------- 1 | %%% Determine total OMPC cost, including bits not depending on d.o.f. 2 | %%% nc is the control horizon. 3 | %%% 4 | %%% Assume model x(k+1)=Ax(k)+Bu(k), y= Cx+Du 5 | %%% 6 | %%% Overall cost 7 | %%%% J = x SX x + 2*x SXC c + c SC c 8 | %% 9 | %%%%% Control law and predictions based on Q, R 10 | %%%%% Regulation case only based on LQR feedback for 11 | %%%%% J =sum x'Qx+u'Ru 12 | %%%% 13 | %%%% [SX,SC,SXC,Spsi]=ompc_cost(A,B,C,D,Q,R,nc) 14 | %%%% 15 | %%%% Code by JA Rossiter (2014) 16 | 17 | % File produced by Anthony Rossiter (University of Sheffield) 18 | % With a creative commons copyright so that users can re-use and edit and redistribute as they please. 19 | % Files are deliberately simple so users can more easily follow the code and edit as required. 20 | % Provided free of charge and thus with no warranty. 21 | 22 | function [SX,SC,SXC,Spsi]=ompc_cost(A,B,C,D,Q,R,nc) 23 | 24 | nx = size(A,1); 25 | nxc=nx*nc; 26 | nu=size(B,2); 27 | nuc=nu*nc; 28 | 29 | %%%%% Feedback loop is of the form u = -Kx+c 30 | %%%%% Find LQR optimal feedback 31 | [K] = dlqr(A,B,Q,R); 32 | Phi=A-B*K; 33 | 34 | %%% Build autonomous model 35 | ID=diag(ones(1,(nc-1)*nu)); 36 | ID=[zeros((nc-1)*nu,nu),ID]; 37 | ID=[ID;zeros(nu,nuc)]; 38 | Psi=[A-B*K,[B,zeros(nx,nuc-nu)];zeros(nuc,nx),ID]; 39 | Gamma=[eye(nx),zeros(nx,nuc)]; 40 | Kz = [K,-eye(nu),zeros(nu,nuc-nu)]; 41 | 42 | %%%% Solve for the cost parameters using lyapunov 43 | W=Psi'*Gamma'*Q*Gamma*Psi+Kz'*R*Kz; 44 | Spsi=dlyap(Psi',W); 45 | 46 | %%% Split cost into parts 47 | %%%% J = [x,c] Spsi [x;c] 48 | %%%% J = x SX x + 2*x SXC c + c SC c 49 | SX=Spsi(1:nx,1:nx); 50 | SXC=Spsi(1:nx,nx+1:end); 51 | SC=Spsi(nx+1:end,nx+1:end); 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Adt.txt: -------------------------------------------------------------------------------- 1 | 1,0.01,0,0,0,0,0,0,-0.0004905000000000001,-1.635e-06,0,0,-4.05752740853708e-11,4.05752740853708e-11,-4.05752740853708e-11,4.05752740853708e-11 2 | 0,1,0,0,0,0,0,0,-0.09810000000000001,-0.0004905000000000001,0,0,-1.589163095061321e-08,1.589163095061321e-08,-1.589163095061321e-08,1.589163095061321e-08 3 | 0,0,1,0.01,0,0,0.0004905000000000001,1.635e-06,0,0,0,0,4.05752740853708e-11,4.05752740853708e-11,-4.05752740853708e-11,-4.05752740853708e-11 4 | 0,0,0,1,0,0,0.09810000000000001,0.0004905000000000001,0,0,0,0,1.589163095061321e-08,1.589163095061321e-08,-1.589163095061321e-08,-1.589163095061321e-08 5 | 0,0,0,0,1,0.01,0,0,0,0,0,0,-3.555801184840882e-08,-3.555801184840882e-08,-3.555801184840882e-08,-3.555801184840882e-08 6 | 0,0,0,0,0,1,0,0,0,0,0,0,-6.62492896954863e-06,-6.62492896954863e-06,-6.62492896954863e-06,-6.62492896954863e-06 7 | 0,0,0,0,0,0,1,0.01,0,0,0,0,4.692184455213431e-07,4.692184455213431e-07,-4.692184455213431e-07,-4.692184455213431e-07 8 | 0,0,0,0,0,0,0,1,0,0,0,0,8.742161642876062e-05,8.742161642876062e-05,-8.742161642876062e-05,-8.742161642876062e-05 9 | 0,0,0,0,0,0,0,0,1,0.01,0,0,4.692184455213431e-07,-4.692184455213431e-07,4.692184455213431e-07,-4.692184455213431e-07 10 | 0,0,0,0,0,0,0,0,0,1,0,0,8.742161642876062e-05,-8.742161642876062e-05,8.742161642876062e-05,-8.742161642876062e-05 11 | 0,0,0,0,0,0,0,0,0,0,1,0.01,1.290619895253161e-08,-1.290619895253161e-08,-1.290619895253161e-08,1.290619895253161e-08 12 | 0,0,0,0,0,0,0,0,0,0,0,1,2.404595951311913e-06,-2.404595951311913e-06,-2.404595951311913e-06,2.404595951311913e-06 13 | 0,0,0,0,0,0,0,0,0,0,0,0,0.6426210983825759,0,0,0 14 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0.6426210983825759,0,0 15 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.6426210983825759,0 16 | 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.6426210983825759 17 | -------------------------------------------------------------------------------- /Y6 MPC Controller/constraint_mats.m: -------------------------------------------------------------------------------- 1 | function [Pc, qc, Sc] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf) 2 | % 3 | % CONSTRAINTS_MATS.M returns the MPC constraints matrices for a system that 4 | % is subject to constraints 5 | % 6 | % Pu*u(k+j|k) <= qu 7 | % Px*x(k+j|k) <= qx 8 | % Pxf*x(k+N|k) <= qxf 9 | % 10 | % That is, the matrices Pc, qc and Sc from 11 | % 12 | % Pc*U(k) <= qc + Sc*x(k) 13 | % 14 | % USAGE: 15 | % 16 | % [Pc,qc,Sc] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf) 17 | % 18 | % where F, G are the prediction matrices obtained from PREDICT_MATS.M 19 | % 20 | % P. Trodden, 2017. 21 | 22 | % input dimension 23 | m = size(Pu,2); 24 | 25 | % state dimension 26 | n = size(F,2); 27 | 28 | % horizon length 29 | N = size(F,1)/n; 30 | 31 | % number of input constraints 32 | ncu = numel(qu); 33 | 34 | % number of state constraints 35 | ncx = numel(qx); 36 | 37 | % number of terminal constraints 38 | ncf = numel(qxf); 39 | 40 | % if state constraints exist, but terminal ones do not, then extend the 41 | % former to the latter 42 | if ncf == 0 && ncx > 0 43 | Pxf = Px; 44 | qxf = qx; 45 | ncf = ncx; 46 | end 47 | 48 | %% Input constraints 49 | 50 | % Build "tilde" (stacked) matrices for constraints over horizon 51 | Pu_tilde = kron(eye(N),Pu); 52 | qu_tilde = kron(ones(N,1),qu); 53 | Scu = zeros(ncu*N,n); 54 | 55 | %% State constraints 56 | 57 | % Build "tilde" (stacked) matrices for constraints over horizon 58 | Px0_tilde = [Px; zeros(ncx*(N-1) + ncf,n)]; 59 | if ncx > 0 60 | Px_tilde = [kron(eye(N-1),Px) zeros(ncx*(N-1),n)]; 61 | else 62 | Px_tilde = zeros(ncx,n*N); 63 | end 64 | Pxf_tilde = [zeros(ncf,n*(N-1)) Pxf]; 65 | Px_tilde = [zeros(ncx,n*N); Px_tilde; Pxf_tilde]; 66 | qx_tilde = [kron(ones(N,1),qx); qxf]; 67 | 68 | %% Final stack 69 | if isempty(Px_tilde) 70 | Pc = Pu_tilde; 71 | qc = qu_tilde; 72 | Sc = Scu; 73 | else 74 | % eliminate x for final form 75 | Pc = [Pu_tilde; Px_tilde*G]; 76 | qc = [qu_tilde; qx_tilde]; 77 | Sc = [Scu; -Px0_tilde - Px_tilde*F]; 78 | end 79 | -------------------------------------------------------------------------------- /Y6 MPC Controller/constraint_mats_cl.m: -------------------------------------------------------------------------------- 1 | function [Pc, qc, Sc] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf) 2 | % 3 | % CONSTRAINTS_MATS.M returns the MPC constraints matrices for a system that 4 | % is subject to constraints 5 | % 6 | % Pu*u(k+j|k) <= qu 7 | % Px*x(k+j|k) <= qx 8 | % Pxf*x(k+N|k) <= qxf 9 | % 10 | % That is, the matrices Pc, qc and Sc from 11 | % 12 | % Pc*U(k) <= qc + Sc*x(k) 13 | % 14 | % USAGE: 15 | % 16 | % [Pc,qc,Sc] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf) 17 | % 18 | % where F, G are the prediction matrices obtained from PREDICT_MATS.M 19 | % 20 | % P. Trodden, 2017. 21 | 22 | % input dimension 23 | m = size(Pu,2); 24 | 25 | % state dimension 26 | n = size(F,2); 27 | 28 | % horizon length 29 | N = size(F,1)/n; 30 | 31 | % number of input constraints 32 | ncu = numel(qu); 33 | 34 | % number of state constraints 35 | ncx = numel(qx); 36 | 37 | % number of terminal constraints 38 | ncf = numel(qxf); 39 | 40 | % if state constraints exist, but terminal ones do not, then extend the 41 | % former to the latter 42 | if ncf == 0 && ncx > 0 43 | Pxf = Px; 44 | qxf = qx; 45 | ncf = ncx; 46 | end 47 | 48 | %% Input constraints 49 | 50 | % Build "tilde" (stacked) matrices for constraints over horizon 51 | Pu_tilde = kron(eye(N),Pu); 52 | qu_tilde = kron(ones(N,1),qu); 53 | Scu = zeros(ncu*N,n); 54 | 55 | %% State constraints 56 | 57 | % Build "tilde" (stacked) matrices for constraints over horizon 58 | Px0_tilde = [Px; zeros(ncx*(N-1) + ncf,n)]; 59 | if ncx > 0 60 | Px_tilde = [kron(eye(N-1),Px) zeros(ncx*(N-1),n)]; 61 | else 62 | Px_tilde = zeros(ncx,n*N); 63 | end 64 | Pxf_tilde = [zeros(ncf,n*(N-1)) Pxf]; 65 | Px_tilde = [zeros(ncx,n*N); Px_tilde; Pxf_tilde]; 66 | qx_tilde = [kron(ones(N,1),qx); qxf]; 67 | 68 | %% Final stack 69 | if isempty(Px_tilde) 70 | Pc = Pu_tilde; 71 | qc = qu_tilde; 72 | Sc = Scu; 73 | else 74 | % eliminate x for final form 75 | Pc = [Pu_tilde; Px_tilde*G]; 76 | qc = [qu_tilde; qx_tilde]; 77 | Sc = [Scu; -Px0_tilde - Px_tilde*F]; 78 | end 79 | -------------------------------------------------------------------------------- /Y6 MPC Controller/ompc_suboptcost.m: -------------------------------------------------------------------------------- 1 | %%% Determine suboptimal MPC cost, including bits not depending on d.o.f. 2 | %%% nc is the control horizon. 3 | %%% Assumes terminal mode not matched to the LQR regulator. 4 | %%% for performance index of J =sum x'Q2 x+u'R2 u 5 | %%% 6 | %%% Assume model x(k+1)=Ax(k)+Bu(k), y= Cx+Du 7 | %%% 8 | %%% Overall cost 9 | %%%% J = x SX x + 2*x SXC c + c SC c 10 | %% 11 | %%%%% Terminal Control law u=-Kx and predictions based on Q2, R2 12 | %%%%% Regulation case only based on LQR feedback for 13 | %%%%% J =sum x'Q2 x+u'R2 u 14 | %%%% 15 | %%%% [SX,SC,SXC,Spsi,K,Psi,Kz]=chap4_suboptcost(A,B,C,D,Q,R,Q2,R2,nc) 16 | %%%% 17 | %%%% Builds an autonomous model Z= Psi Z, u = -Kz Z Z=[x;cfut]; 18 | %%%% 19 | %%%% Code by JA Rossiter (2014) 20 | 21 | % File produced by Anthony Rossiter (University of Sheffield) 22 | % With a creative commons copyright so that users can re-use and edit and redistribute as they please. 23 | % Files are deliberately simple so users can more easily follow the code and edit as required. 24 | % Provided free of charge and thus with no warranty. 25 | 26 | 27 | function [SX,SC,SXC,Spsi,K,Psi,Kz]=chap4_suboptcost(A,B,C,D,Q,R,Q2,R2,nc) 28 | 29 | nx = size(A,1); 30 | nxc=nx*nc; 31 | nu=size(B,2); 32 | nuc=nu*nc; 33 | 34 | %%%%% Feedback loop is of the form u = -Kx+c 35 | %%%%% Find terminal mode feedback (uses LQR based on Q2, R2) 36 | [K] = dlqr(A,B,Q2,R2); 37 | Phi=A-B*K; 38 | 39 | %%% Build autonomous model 40 | ID=diag(ones(1,(nc-1)*nu)); 41 | ID=[zeros((nc-1)*nu,nu),ID]; 42 | ID=[ID;zeros(nu,nuc)]; 43 | Psi=[A-B*K,[B,zeros(nx,nuc-nu)];zeros(nuc,nx),ID]; 44 | Gamma=[eye(nx),zeros(nx,nuc)]; 45 | Kz = [K,-eye(nu),zeros(nu,nuc-nu)]; 46 | 47 | %%%% Solve for the cost parameters using lyapunov 48 | W=Psi'*Gamma'*Q*Gamma*Psi+Kz'*R*Kz; 49 | Spsi=dlyap(Psi',W); 50 | 51 | %%% Split cost into parts 52 | %%%% J = [x,c] Spsi [x;c] 53 | %%%% J = x SX x + 2*x SXC c + c SC c 54 | SX=Spsi(1:nx,1:nx); 55 | SXC=Spsi(1:nx,nx+1:end); 56 | SC=Spsi(nx+1:end,nx+1:end); 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /Y6 MPC Controller/findmas.m: -------------------------------------------------------------------------------- 1 | %%% Find a MAS given the following dynamics 2 | %%% F x <=t 3 | %%% Process is x(k+1)=Ax(k) 4 | %%% Constraints at each sample are Cx <=f 5 | %%% 6 | %%% Uses a simple while loop and simple method based 7 | %%% on gradually increasing the number of rows of a matrix inequality 8 | %%% does not remove redundant constraints 9 | %%% F=[C;CA;CA^2;...] <=[f;f;f;...] 10 | %%% 11 | %%% [F,t]=findmas(A,C,f) 12 | 13 | % File produced by Anthony Rossiter (University of Sheffield) 14 | % With a creative commons copyright so that users can re-use and edit and redistribute as they please. 15 | % Files are deliberately simple so users can more easily follow the code and edit as required. 16 | % Provided free of charge and thus with no warranty. 17 | 18 | function [F,t]=findmas(A,C,f) 19 | 20 | %%%% initial set for k=0; 21 | F=C; 22 | t=f; 23 | val=zeros(size(f)); 24 | An=C; 25 | nc=size(C,1); %%% number of constraints in each sample 26 | Inc=eye(nc); 27 | 28 | %%% Switch of display 29 | opt = optimset('linprog'); 30 | opt.Diagnostics='off'; %%%%% Switches of unwanted MATLAB displays 31 | %opt.LargeScale='off'; %%%%% However no warning of infeasibility 32 | opt.Display='off'; 33 | 34 | cont=1; 35 | while cont==1; 36 | An=An*A; %%% forms new block An=C*A^n 37 | 38 | %%% inequalities to maximise - check each row of new constraints 39 | %%% e(j)^T An x <= t(j) 40 | for j=1:nc; 41 | vec=-Inc(j,:)*An; 42 | [x,vv,exitflag]=linprog(vec,F,t,[],[],[],[],[],opt); 43 | if exitflag==1; val(j)=vv; 44 | elseif exitflag==-3; %%% indicates solution unbounded 45 | val(j)=-f(j)*2; %% marks this row as needed 46 | end 47 | end 48 | 49 | %%% Extra rows are needed if any values of val exceed f 50 | %%% so add them in 51 | if any(-val>f); 52 | 53 | F=[F;An]; %% Add extra block to F 54 | t=[t;f]; %% Add extra block to t 55 | else 56 | cont=0; %%% finish loop as all new rows redundant 57 | end 58 | end 59 | 60 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Linear_Quadratic_Gaussian.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | class LQG: 5 | 6 | def __init__(self,Adt,Bdt,Cdt,Ddt,Kdt,Kidt,Ldt,U_e): 7 | 8 | self.Adt = Adt 9 | self.Bdt = Bdt 10 | self.Cdt = Cdt 11 | self.Ddt = Ddt 12 | self.Kdt = Kdt 13 | self.Kidt = Kidt 14 | self.Ldt = Ldt 15 | self.U_e = U_e[:].reshape(self.Bdt.shape[1],1) 16 | 17 | self.U = np.zeros((self.Bdt.shape[1],1)) 18 | self.prevU = np.zeros((self.Bdt.shape[1],1)) 19 | 20 | self.Xest = np.zeros((self.Adt.shape[0],1)) 21 | self.prevXest = np.zeros((self.Adt.shape[0],1)) 22 | 23 | self.Xe = np.zeros((self.Cdt.shape[0],1)) 24 | self.prevXe = np.zeros((self.Cdt.shape[0],1)) 25 | 26 | self.Ref = np.zeros((self.Cdt.shape[0],1)) 27 | self.Y = np.zeros((self.Cdt.shape[0],1)) 28 | self.e = np.zeros((self.Cdt.shape[0],1)) 29 | 30 | def calculate(self,prevU,Y,Ref,Linear): 31 | 32 | self.Y = Y[:] 33 | self.prevU = prevU[:] 34 | self.Ref = Ref[:] 35 | 36 | if Linear == True: 37 | self.Xest = self.Adt @ self.prevXest + self.Bdt @ (self.prevU) # KF Linear Prediction 38 | self.e = self.Y - self.Xest[[0,2,4,10]] 39 | self.Xest = self.Xest + self.Ldt @ self.e 40 | 41 | self.Xe = self.prevXe + (self.Ref - self.Xest[[0,2,4,10]]) # Integrator 42 | self.U = - (self.Kdt @ self.Xest) - (self.Kidt @ self.Xe) 43 | else: 44 | self.Xest = self.Adt @ self.prevXest + self.Bdt @ (self.prevU-self.U_e) # KF Linear Prediction 45 | self.e = self.Y - self.Xest[[0,2,4,10]] 46 | self.Xest = self.Xest + self.Ldt @ self.e 47 | 48 | self.Xe = self.prevXe + (self.Ref - self.Xest[[0,2,4,10]]) # Integrator 49 | self.U = np.clip(self.U_e - (self.Kdt @ self.Xest) - (self.Kidt @ self.Xe),0,800) 50 | 51 | self.prevXe = self.Xe 52 | self.prevXest = self.Xest 53 | 54 | return self.U[:] 55 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Quad_Dynamics_V.m: -------------------------------------------------------------------------------- 1 | function [dX] = Quad_Dynamics_V(t,X,U) 2 | %% Mass of the Multirotor in Kilograms as taken from the CAD 3 | M = 1.157685; 4 | g = 9.81; 5 | 6 | %% Dimensions of Multirotor 7 | L = 0.16319; % along X-axis and Y-axis Distance from left and right motor pair to center of mass 8 | 9 | %% Mass Moment of Inertia as Taken from the CAD 10 | % Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 11 | Ixx = 1.129E+05/(1000*10000); 12 | Iyy = 1.129E+05/(1000*10000); 13 | Izz = 2.033E+05/(1000*10000); 14 | 15 | %% Motor Thrust and Torque Constants (To be determined experimentally) 16 | Ktau = 7.708e-10 * 2; 17 | Kthrust = 1.812e-07; 18 | Kthrust2 = 0.0007326; 19 | Mtau = (1/44.22); 20 | Ku = 515.5; 21 | 22 | %% Air resistance damping coeeficients 23 | Dxx = 0.01212; 24 | Dyy = 0.01212; 25 | Dzz = 0.0648; 26 | 27 | %% X = [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4] 28 | X = num2cell(X); 29 | [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4] = X{:}; 30 | 31 | %% Initialise Output and state vectors 32 | dX = zeros(16,1); 33 | 34 | %% Motor Forces and Torques 35 | F = zeros(4,1); 36 | T = zeros(4,1); 37 | 38 | F = Kthrust*[w1^2;w2^2;w3^2;w4^2] + Kthrust2*[w1;w2;w3;w4]; 39 | T = [Ktau,-Ktau,-Ktau,Ktau]*[w1^2;w2^2;w3^2;w4^2]; 40 | 41 | Fn = sum(F); 42 | Tn = sum(T); 43 | 44 | %% First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 45 | dX([1,3,5]) = [xdot;ydot;zdot]; 46 | 47 | dX([7,9,11]) = [1,(sin(phi)*tan(theta)),cos(phi)*tan(theta);0,cos(phi),-sin(phi);0,sin(phi)/cos(theta),cos(phi)/cos(theta)] * [p;q;r]; 48 | 49 | %% Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 50 | dX([2,4,6]) = (Fn/M)*[cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi);cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi);-cos(phi)*cos(theta)] + [0;0;g] - [Dxx/M,0,0;0,Dyy/M,0;0,0,Dzz/M]*[xdot;ydot;zdot]; 51 | 52 | dX([8,10,12]) = [(L/Ixx)*((F(1)+F(2)) - (F(3)+F(4)));(L/Iyy)*((F(1)+F(3)) - (F(2)+F(4)));Tn/Izz] - [(((Izz-Iyy)/Ixx)*(r*q));(((Izz-Ixx)/Iyy)*(p*r));(((Iyy-Ixx)/Izz)*(p*q))]; 53 | 54 | %% Motor Dynamics: dX = [w1dot w2dot w3dot w4dot], U = PWM Pulse Width between (0-1000)us 55 | dX(13:end) = -(1/Mtau)*[w1;w2;w3;w4] + Ku*U; 56 | end -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MPC_code/Draw_MPC_tracking_v1.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 2 | 3 | 4 | set(0,'DefaultAxesFontName', 'Times New Roman') 5 | set(0,'DefaultAxesFontSize', 12) 6 | 7 | line_width = 1.5; 8 | fontsize_labels = 14; 9 | 10 | %-------------------------------------------------------------------------- 11 | %-----------------------Simulate robots ----------------------------------- 12 | %-------------------------------------------------------------------------- 13 | x_r_1 = []; 14 | y_r_1 = []; 15 | 16 | r = rob_diam/2; % obstacle radius 17 | ang=0:0.005:2*pi; 18 | xp=r*cos(ang); 19 | yp=r*sin(ang); 20 | 21 | figure(500) 22 | % Animate the robot motion 23 | %figure;%('Position',[200 200 1280 720]); 24 | set(gcf,'PaperPositionMode','auto') 25 | set(gcf, 'Color', 'w'); 26 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 27 | 28 | for k = 1:size(xx,2) 29 | plot([0 12],[1 1],'-g','linewidth',1.2);hold on % plot the reference trajectory 30 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 31 | x_r_1 = [x_r_1 x1]; 32 | y_r_1 = [y_r_1 y1]; 33 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 34 | if k < size(xx,2) % plot prediction 35 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 36 | end 37 | 38 | plot(x1,y1,'-sk','MarkerSize',25)% plot robot position 39 | hold off 40 | %figure(500) 41 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 42 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 43 | axis([-1 16 -0.5 1.5]) 44 | pause(0.2) 45 | box on; 46 | grid on 47 | %aviobj = addframe(aviobj,gcf); 48 | drawnow 49 | % for video generation 50 | F(k) = getframe(gcf); % to get the current frame 51 | end 52 | close(gcf) 53 | %viobj = close(aviobj) 54 | %video = VideoWriter('exp.avi','Uncompressed AVI'); 55 | 56 | % video = VideoWriter('exp.avi','Motion JPEG AVI'); 57 | % video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 58 | % open(video) 59 | % writeVideo(video,F) 60 | % close (video) 61 | 62 | figure 63 | subplot(211) 64 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.2 0.8]) 65 | ylabel('v (rad/s)') 66 | grid on 67 | subplot(212) 68 | stairs(t,u_cl(:,2),'r','linewidth',1.5); %axis([0 t(end) -0.85 0.85]) 69 | xlabel('time (seconds)') 70 | ylabel('\omega (rad/s)') 71 | grid on 72 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Quad_Dynamics.m: -------------------------------------------------------------------------------- 1 | function [dX] = Quad_Dynamics(t,X,U) 2 | 3 | %% Mass of the Multirotor in Kilograms as taken from the CAD 4 | 5 | M = 0.857945; 6 | g = 9.81; 7 | 8 | %% Dimensions of Multirotor 9 | 10 | L = 0.16319; % along X-axis and Y-axis Distance from left and right motor pair to center of mass 11 | 12 | %% Mass Moment of Inertia as Taken from the CAD 13 | % Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 14 | 15 | Ixx = 1.061E+05/(1000*10000); 16 | Iyy = 1.061E+05/(1000*10000); 17 | Izz = 2.011E+05/(1000*10000); 18 | 19 | %% Motor Thrust and Torque Constants (To be determined experimentally) 20 | 21 | Ktau = 7.708e-10 * 2; 22 | Kthrust = 1.812e-07; 23 | Kthrust2 = 0.0007326; 24 | Mtau = 1/44.22; 25 | Ku = 515.5; 26 | 27 | %% Air resistance damping coeeficients 28 | 29 | Dxx = 0.01212; 30 | Dyy = 0.01212; 31 | Dzz = 0.0648; 32 | 33 | %% X = [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4] 34 | 35 | x = X(1); 36 | xdot = X(2); 37 | y = X(3); 38 | ydot = X(4); 39 | z = X(5); 40 | zdot = X(6); 41 | 42 | phi = X(7); 43 | p = X(8); 44 | theta = X(9); 45 | q = X(10); 46 | psi = X(11); 47 | r = X(12); 48 | 49 | w1 = X(13); 50 | w2 = X(14); 51 | w3 = X(15); 52 | w4 = X(16); 53 | 54 | %% Motor Forces and Torques 55 | 56 | F1 = Kthrust*w1*w1 + Kthrust2*w1; 57 | F2 = Kthrust*w2*w2 + Kthrust2*w2; 58 | F3 = Kthrust*w3*w3 + Kthrust2*w3; 59 | F4 = Kthrust*w4*w4 + Kthrust2*w4; 60 | 61 | T1 = Ktau*w1*w1; 62 | T2 = -Ktau*w2*w2; 63 | T3 = -Ktau*w3*w3; 64 | T4 = Ktau*w4*w4; 65 | 66 | Fn = F1+F2+F3+F4; 67 | Tn = T1+T2+T3+T4; 68 | 69 | %% First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 70 | 71 | dX1 = xdot; 72 | dX3 = ydot; 73 | dX5 = zdot; 74 | dX7 = p + q*(sin(phi)*tan(theta)) + r*(cos(phi)*tan(theta)); 75 | dX9 = q*cos(phi) - r*sin(phi); 76 | dX11 = q*(sin(phi)/cos(theta)) + r*(cos(phi)/cos(theta)); 77 | 78 | %% Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 79 | 80 | dX2 = Fn/M*(cos(phi)*sin(theta)*cos(psi)) + Fn/M*(sin(phi)*sin(psi)) - (Dxx/M)*xdot; 81 | dX4 = Fn/M*(cos(phi)*sin(theta)*sin(psi)) - Fn/M*(sin(phi)*cos(psi)) - (Dyy/M)*ydot; 82 | dX6 = g - Fn/M*(cos(phi)*cos(theta)) -(Dzz/M)*zdot; 83 | 84 | dX8 = (L/Ixx)*((F1+F2) - (F3+F4)) - (((Izz-Iyy)/Ixx)*(r*q)); 85 | dX10 = (L/Iyy)*((F1+F3) - (F2+F4)) - (((Izz-Ixx)/Iyy)*(p*r)); 86 | dX12 = Tn/Izz - (((Iyy-Ixx)/Izz)*(p*q)); 87 | 88 | %% Motor Dynamics: dX = [w1dot w2dot w3dot w4dot], U = Pulse Width of the pwm signal 0-1000 89 | 90 | dX13 = -(1/Mtau)*w1 + Ku*U(1); 91 | dX14 = -(1/Mtau)*w2 + Ku*U(2); 92 | dX15 = -(1/Mtau)*w3 + Ku*U(3); 93 | dX16 = -(1/Mtau)*w4 + Ku*U(4); 94 | 95 | dX = [dX1;dX2;dX3;dX4;dX5;dX6;dX7;dX8;dX9;dX10;dX11;dX12;dX13;dX14;dX15;dX16]; 96 | end -------------------------------------------------------------------------------- /X4 LQG Controller/12States/Quad_Dynamics.m: -------------------------------------------------------------------------------- 1 | function [dX] = Quad_Dynamics(t,X,U) 2 | %% Mass of the Multirotor in Kilograms as taken from the CAD 3 | 4 | M = 1.157685 ; 5 | g = 9.81; 6 | 7 | %% Dimensions of Multirotor 8 | 9 | L = 0.16319; % along X-axis and Y-axis Distance from left and right motor pair to center of mass 10 | 11 | %% Mass Moment of Inertia as Taken from the CAD 12 | % Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 13 | Ixx = 1.129E+05/(1000*10000); 14 | Iyy = 1.129E+05/(1000*10000); 15 | Izz = 2.033E+05/(1000*10000); 16 | 17 | %% Motor Thrust and Torque Constants (To be determined experimentally) 18 | Ktau = 7.708e-10 * 2; 19 | Kthrust = 1.812e-07; 20 | Kthrust2 = 0.0007326; 21 | Mtau = (1/44.22); 22 | Ku = 515.5; 23 | 24 | %% Air resistance damping coeeficients 25 | Dxx = 0.01212; 26 | Dyy = 0.01212; 27 | Dzz = 0.0648; 28 | 29 | %% X = [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4] 30 | x = X(1); 31 | xdot = X(2); 32 | y = X(3); 33 | ydot = X(4); 34 | z = X(5); 35 | zdot = X(6); 36 | 37 | phi = X(7); 38 | p = X(8); 39 | theta = X(9); 40 | q = X(10); 41 | psi = X(11); 42 | r = X(12); 43 | 44 | w1 = X(13); 45 | w2 = X(14); 46 | w3 = X(15); 47 | w4 = X(16); 48 | 49 | %% Initialise Outputs 50 | dX = zeros(16,1); 51 | 52 | %% Motor Dynamics: dX = [w1dot w2dot w3dot w4dot], U = Pulse Width of the pwm signal 0-1000% 53 | dX(13) = -(1/Mtau)*w1 + Ku*U(1); 54 | dX(14) = -(1/Mtau)*w2 + Ku*U(2); 55 | dX(15) = -(1/Mtau)*w3 + Ku*U(3); 56 | dX(16) = -(1/Mtau)*w4 + Ku*U(4); 57 | 58 | %% Motor Forces and Torques 59 | F = zeros(4,1); 60 | T = zeros(4,1); 61 | 62 | F(1) = Kthrust*(w1^2) + Kthrust2*w1; 63 | F(2) = Kthrust*(w2^2) + Kthrust2*w2; 64 | F(3) = Kthrust*(w3^2) + Kthrust2*w3; 65 | F(4) = Kthrust*(w4^2) + Kthrust2*w4; 66 | 67 | T(1) = Ktau*(w1^2); 68 | T(2) = -Ktau*(w2^2); 69 | T(3) = -Ktau*(w3^2); 70 | T(4) = Ktau*(w4^2); 71 | 72 | Fn = sum(F); 73 | Tn = sum(T); 74 | 75 | %% First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 76 | dX(1) = xdot; 77 | dX(3) = ydot; 78 | dX(5) = zdot; 79 | dX(7) = p + q*(sin(phi)*tan(theta)) + r*(cos(phi)*tan(theta)); 80 | dX(9) = q*cos(phi) - r*sin(phi); 81 | dX(11) = q*(sin(phi)/cos(theta)) + r*(cos(phi)/cos(theta)); 82 | 83 | %% Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 84 | dX(2) = Fn/M*(cos(phi)*sin(theta)*cos(psi)) + Fn/M*(sin(phi)*sin(psi)) - (Dxx/M)*xdot; 85 | dX(4) = Fn/M*(cos(phi)*sin(theta)*sin(psi)) - Fn/M*(sin(phi)*cos(psi)) - (Dyy/M)*ydot; 86 | dX(6) = g - Fn/M*(cos(phi)*cos(theta)) -(Dzz/M)*zdot; 87 | 88 | dX(8) = (L/Ixx)*((F(1)+F(2)) - (F(3)+F(4))) - (((Izz-Iyy)/Ixx)*(r*q)); 89 | dX(10) = (L/Iyy)*((F(1)+F(3)) - (F(2)+F(4))) - (((Izz-Ixx)/Iyy)*(p*r)); 90 | dX(12) = Tn/Izz - (((Iyy-Ixx)/Izz)*(p*q)); 91 | 92 | end -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Quad_Dynamics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import cmath 4 | 5 | # Initialise Outputs 6 | dX = np.zeros(16) 7 | 8 | # Motor Forces and Torques 9 | F = np.zeros(4) 10 | T = np.zeros(4) 11 | 12 | # Mass of the Multirotor in Kilograms as taken from the CAD 13 | M = 0.799406 14 | g = 9.81 15 | 16 | # Dimensions of Multirotor 17 | L = 0.269/2 # along X-axis and Y-axis Distance from left and right motor pair to center of mass 18 | 19 | # Mass Moment of Inertia as Taken from the CAD 20 | # Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 21 | Ixx = 6.609E+04/(1000*10000) 22 | Iyy = 6.610E+04/(1000*10000) 23 | Izz = 1.159E+05/(1000*10000) 24 | 25 | # Motor Thrust and Torque Constants (To be determined experimentally) 26 | Ktau = 7.708e-10 27 | Kthrust = 1.812e-07 28 | Kthrust2 = 0.0007326 29 | Mtau = (1/44.22) 30 | Ku = 515.5*Mtau 31 | 32 | # Air resistance damping coeeficients 33 | Dxx = 0.01212 34 | Dyy = 0.01212 35 | Dzz = 0.0648 36 | 37 | def Quad_Dynamics(t,X,U,a): 38 | 39 | [x,xdot,y,ydot,z,zdot,phi,p,theta,q,psi,r,w1,w2,w3,w4] = X 40 | 41 | # Motor Forces and Torques 42 | F[0] = Kthrust*w1*w1 + Kthrust2*w1 43 | F[1] = Kthrust*w2*w2 + Kthrust2*w2 44 | F[2] = Kthrust*w3*w3 + Kthrust2*w3 45 | F[3] = Kthrust*w4*w4 + Kthrust2*w4 46 | 47 | T[0] = -Ktau*w1*w1 48 | T[1] = Ktau*w2*w2 49 | T[2] = Ktau*w3*w3 50 | T[3] = -Ktau*w4*w4 51 | 52 | Fn = sum(F) 53 | Tn = sum(T) 54 | 55 | # First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 56 | dX[0] = xdot 57 | dX[2] = ydot 58 | dX[4] = zdot 59 | dX[6] = p + q*(cmath.sin(phi).real*cmath.tan(theta).real) + r*(cmath.cos(phi).real*cmath.tan(theta).real) 60 | dX[8] = q*cmath.cos(phi).real - r*cmath.sin(phi).real 61 | dX[10] = q*(cmath.sin(phi).real/cmath.cos(theta).real) + r*(cmath.cos(phi).real/cmath.cos(theta).real) 62 | 63 | # Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 64 | dX[1] = Fn/M*(cmath.cos(phi).real*cmath.sin(theta).real*cmath.cos(psi).real) + Fn/M*(cmath.sin(phi).real*cmath.sin(psi).real) - (Dxx/M)*xdot 65 | dX[3] = Fn/M*(cmath.cos(phi).real*cmath.sin(theta).real*cmath.sin(psi).real) - Fn/M*(cmath.sin(phi).real*cmath.cos(psi).real) - (Dyy/M)*ydot 66 | dX[5] = Fn/M*(cmath.cos(phi).real*cmath.cos(theta).real) - g - (Dzz/M)*zdot 67 | 68 | dX[7] = (L/Ixx)*(F[0]+F[1] - F[2]+F[3]) - (((Izz-Iyy)/Ixx)*(r*q)) 69 | dX[9] = (L/Iyy)*(F[0]+F[2] - F[1]+F[3]) - (((Izz-Ixx)/Iyy)*(p*r)) 70 | dX[11] = Tn/Izz - (((Iyy-Ixx)/Izz)*(p*q)) 71 | 72 | # Motor Dynamics: dX = [w1dot w2dot w3dot w4dot], U = Pulse Width of the pwm signal 0-1000 73 | dX[12] = -(1/Mtau)*w1 + (Ku/Mtau)*U[0] 74 | dX[13] = -(1/Mtau)*w2 + (Ku/Mtau)*U[1] 75 | dX[14] = -(1/Mtau)*w3 + (Ku/Mtau)*U[2] 76 | dX[15] = -(1/Mtau)*w4 + (Ku/Mtau)*U[3] 77 | 78 | return dX 79 | -------------------------------------------------------------------------------- /Y6 MPC Controller/ompc_constraints.m: -------------------------------------------------------------------------------- 1 | %%% Simulation of dual mode optimal predictive control - REGULATION CASE 2 | %%% NO INTEGRAL ACTION/OFFSET computation 3 | %%% 4 | %% [J,x,y,u,c,KSOMPC] = ompc_simulate_constraints(A,B,C,D,nc,Q,R,Q2,R2,x0,runtime,umin,umax,Kxmax,xmax) 5 | %% 6 | %% Q, R denote the weights in the actual cost function 7 | %% Q2, R2 are the weights used to find the terminal mode LQR feedback 8 | %% nc is the control horizon 9 | %% A, B,C,D are the state space model parameters 10 | %% x0 is the initial condition for the simulation 11 | %% J is the predicted cost at each sample 12 | %% c is the optimised perturbation at each sample 13 | %% x,y,u are states, outputs and inputs 14 | %% KSOMPC unconstrained feedback law 15 | %% 16 | %% Adds in constraint handling with constraints 17 | %% umin 1e-2 && mpciter < sim_tim / T) 116 | args.p = [x0;xs]; % set the values of the parameters vector 117 | args.x0 = reshape(u0',2*N,1); % initial value of the optimization variables 118 | %tic 119 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 120 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 121 | %toc 122 | u = reshape(full(sol.x)',2,N)'; 123 | ff_value = ff(u',args.p); % compute OPTIMAL solution TRAJECTORY 124 | xx1(:,1:3,mpciter+1)= full(ff_value)'; 125 | 126 | u_cl= [u_cl ; u(1,:)]; 127 | t(mpciter+1) = t0; 128 | [t0, x0, u0] = shift(T, t0, x0, u,f); % get the initialization of the next optimization step 129 | 130 | xx(:,mpciter+2) = x0; 131 | mpciter 132 | mpciter = mpciter + 1; 133 | end; 134 | main_loop_time = toc(main_loop); 135 | ss_error = norm((x0-xs),2) 136 | average_mpc_time = main_loop_time/(mpciter+1) 137 | 138 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) % a drawing function 139 | 140 | 141 | -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MPC_code/Sim_2_MPC_Robot_PS_mul_shooting.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Multiple shooting 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 100; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include the initial state and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % make the decision variable one column vector 49 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 50 | 51 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 52 | 53 | opts = struct; 54 | opts.ipopt.max_iter = 2000; 55 | opts.ipopt.print_level =0;%0,3 56 | opts.print_time = 0; 57 | opts.ipopt.acceptable_tol =1e-8; 58 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 59 | 60 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 61 | 62 | args = struct; 63 | 64 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 65 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 66 | 67 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 68 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 69 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 70 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 71 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 72 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 73 | 74 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 75 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 76 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 77 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 78 | %---------------------------------------------- 79 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 80 | 81 | 82 | % THE SIMULATION LOOP SHOULD START FROM HERE 83 | %------------------------------------------- 84 | t0 = 0; 85 | x0 = [0 ; 0 ; 0.0]; % initial condition. 86 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 87 | 88 | xx(:,1) = x0; % xx contains the history of states 89 | t(1) = t0; 90 | 91 | u0 = zeros(N,2); % two control inputs for each robot 92 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 93 | 94 | sim_tim = 20; % Maximum simulation time 95 | 96 | % Start MPC 97 | mpciter = 0; 98 | xx1 = []; 99 | u_cl=[]; 100 | 101 | % the main simulaton loop... it works as long as the error is greater 102 | % than 10^-6 and the number of mpc steps is less than its maximum 103 | % value. 104 | main_loop = tic; 105 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 106 | args.p = [x0;xs]; % set the values of the parameters vector 107 | % initial value of the optimization variables 108 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 109 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 110 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 111 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 112 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 113 | u_cl= [u_cl ; u(1,:)]; 114 | t(mpciter+1) = t0; 115 | % Apply the control and shift the solution 116 | [t0, x0, u0] = shift(T, t0, x0, u,f); 117 | xx(:,mpciter+2) = x0; 118 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 119 | % Shift trajectory to initialize the next step 120 | X0 = [X0(2:end,:);X0(end,:)]; 121 | mpciter 122 | mpciter = mpciter + 1; 123 | end; 124 | main_loop_time = toc(main_loop); 125 | ss_error = norm((x0-xs),2) 126 | average_mpc_time = main_loop_time/(mpciter+1) 127 | 128 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 129 | 130 | 131 | -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MPC_code/Sim_3_MPC_Robot_PS_obs_avoid_mul_sh.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Multiple shooting + obstacle avoidance 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 10; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include at the initial state of the robot and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % Add constraints for collision avoidance 49 | obs_x = 0.5; % meters 50 | obs_y = 0.5; % meters 51 | obs_diam = 0.3; % meters 52 | for k = 1:N+1 % box constraints due to the map margins 53 | g = [g ; -sqrt((X(1,k)-obs_x)^2+(X(2,k)-obs_y)^2) + (rob_diam/2 + obs_diam/2)]; 54 | end 55 | % make the decision variable one column vector 56 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 57 | 58 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 59 | 60 | opts = struct; 61 | opts.ipopt.max_iter = 100; 62 | opts.ipopt.print_level =0;%0,3 63 | opts.print_time = 0; 64 | opts.ipopt.acceptable_tol =1e-8; 65 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 66 | 67 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 68 | 69 | args = struct; 70 | args.lbg(1:3*(N+1)) = 0; % equality constraints 71 | args.ubg(1:3*(N+1)) = 0; % equality constraints 72 | 73 | args.lbg(3*(N+1)+1 : 3*(N+1)+ (N+1)) = -inf; % inequality constraints 74 | args.ubg(3*(N+1)+1 : 3*(N+1)+ (N+1)) = 0; % inequality constraints 75 | 76 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 77 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 78 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 79 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 80 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 81 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 82 | 83 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 84 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 85 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 86 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 87 | %---------------------------------------------- 88 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 89 | 90 | 91 | % THE SIMULATION LOOP SHOULD START FROM HERE 92 | %------------------------------------------- 93 | t0 = 0; 94 | x0 = [0 ; 0 ; 0.0]; % initial condition. 95 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 96 | 97 | xx(:,1) = x0; % xx contains the history of states 98 | t(1) = t0; 99 | 100 | u0 = zeros(N,2); % two control inputs for each robot 101 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 102 | 103 | sim_tim = 20; % Maximum simulation time 104 | 105 | % Start MPC 106 | mpciter = 0; 107 | xx1 = []; 108 | u_cl=[]; 109 | 110 | % the main simulaton loop... it works as long as the error is greater 111 | % than 10^-6 and the number of mpc steps is less than its maximum 112 | % value. 113 | main_loop = tic; 114 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 115 | args.p = [x0;xs]; % set the values of the parameters vector 116 | % initial value of the optimization variables 117 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 118 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 119 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 120 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 121 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 122 | u_cl= [u_cl ; u(1,:)]; 123 | t(mpciter+1) = t0; 124 | % Apply the control and shift the solution 125 | [t0, x0, u0] = shift(T, t0, x0, u,f); 126 | xx(:,mpciter+2) = x0; 127 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 128 | % Shift trajectory to initialize the next step 129 | X0 = [X0(2:end,:);X0(end,:)]; 130 | mpciter 131 | mpciter = mpciter + 1; 132 | end; 133 | main_loop_time = toc(main_loop); 134 | ss_error = norm((x0-xs),2) 135 | average_mpc_time = main_loop_time/(mpciter+1) 136 | 137 | Draw_MPC_PS_Obstacles (t,xx,xx1,u_cl,xs,N,rob_diam,obs_x,obs_y,obs_diam) 138 | 139 | 140 | -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MPC_code/Sim_4_MPC_Robot_tracking_mul_shooting.m: -------------------------------------------------------------------------------- 1 | % Trajectory Tracking + Multiple shooting 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.5; %[s] 11 | N = 8; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | %P = SX.sym('P',n_states + n_states); 27 | P = SX.sym('P',n_states + N*(n_states+n_controls)); 28 | % parameters (which include the initial state and the reference along the 29 | % predicted trajectory (reference states and reference controls)) 30 | 31 | X = SX.sym('X',n_states,(N+1)); 32 | % A vector that represents the states over the optimization problem. 33 | 34 | obj = 0; % Objective function 35 | g = []; % constraints vector 36 | 37 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 1;Q(3,3) = 0.5; % weighing matrices (states) 38 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 39 | 40 | st = X(:,1); % initial state 41 | g = [g;st-P(1:3)]; % initial condition constraints 42 | for k = 1:N 43 | st = X(:,k); con = U(:,k); 44 | %obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 45 | obj = obj+(st-P(5*k-1:5*k+1))'*Q*(st-P(5*k-1:5*k+1)) + ... 46 | (con-P(5*k+2:5*k+3))'*R*(con-P(5*k+2:5*k+3)) ; % calculate obj 47 | % the number 5 is (n_states+n_controls) 48 | st_next = X(:,k+1); 49 | f_value = f(st,con); 50 | st_next_euler = st+ (T*f_value); 51 | g = [g;st_next-st_next_euler]; % compute constraints 52 | end 53 | % make the decision variable one column vector 54 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 55 | 56 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 57 | 58 | opts = struct; 59 | opts.ipopt.max_iter = 2000; 60 | opts.ipopt.print_level =0;%0,3 61 | opts.print_time = 0; 62 | opts.ipopt.acceptable_tol =1e-8; 63 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 64 | 65 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 66 | 67 | args = struct; 68 | 69 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 70 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 71 | 72 | args.lbx(1:3:3*(N+1),1) = -20; %state x lower bound % new - adapt the bound 73 | args.ubx(1:3:3*(N+1),1) = 20; %state x upper bound % new - adapt the bound 74 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 75 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 76 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 77 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 78 | 79 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 80 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 81 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 82 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 83 | %---------------------------------------------- 84 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 85 | 86 | 87 | % THE SIMULATION LOOP SHOULD START FROM HERE 88 | %------------------------------------------- 89 | t0 = 0; 90 | x0 = [0 ; 0 ; 0.0]; % initial condition. 91 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 92 | 93 | xx(:,1) = x0; % xx contains the history of states 94 | t(1) = t0; 95 | 96 | u0 = zeros(N,2); % two control inputs for each robot 97 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 98 | 99 | sim_tim = 30; % Maximum simulation time 100 | 101 | % Start MPC 102 | mpciter = 0; 103 | xx1 = []; 104 | u_cl=[]; 105 | 106 | % the main simulaton loop... it works as long as the error is greater 107 | % than 10^-6 and the number of mpc steps is less than its maximum 108 | % value. 109 | main_loop = tic; 110 | while(mpciter < sim_tim / T) % new - condition for ending the loop 111 | current_time = mpciter*T; %new - get the current time 112 | % args.p = [x0;xs]; % set the values of the parameters vector 113 | %---------------------------------------------------------------------- 114 | args.p(1:3) = x0; % initial condition of the robot posture 115 | for k = 1:N %new - set the reference to track 116 | t_predict = current_time + (k-1)*T; % predicted time instant 117 | x_ref = 0.5*t_predict; y_ref = 1; theta_ref = 0; 118 | u_ref = 0.5; omega_ref = 0; 119 | if x_ref >= 12 % the trajectory end is reached 120 | x_ref = 12; y_ref = 1; theta_ref = 0; 121 | u_ref = 0; omega_ref = 0; 122 | end 123 | args.p(5*k-1:5*k+1) = [x_ref, y_ref, theta_ref]; 124 | args.p(5*k+2:5*k+3) = [u_ref, omega_ref]; 125 | end 126 | %---------------------------------------------------------------------- 127 | % initial value of the optimization variables 128 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 129 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 130 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 131 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 132 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 133 | u_cl= [u_cl ; u(1,:)]; 134 | t(mpciter+1) = t0; 135 | % Apply the control and shift the solution 136 | [t0, x0, u0] = shift(T, t0, x0, u,f); 137 | xx(:,mpciter+2) = x0; 138 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 139 | % Shift trajectory to initialize the next step 140 | X0 = [X0(2:end,:);X0(end,:)]; 141 | mpciter 142 | mpciter = mpciter + 1; 143 | end; 144 | main_loop_time = toc(main_loop); 145 | average_mpc_time = main_loop_time/(mpciter+1) 146 | 147 | Draw_MPC_tracking_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 148 | 149 | 150 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/Planning&ControlSim.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | import time 4 | import math 5 | 6 | import numpy as np 7 | import numba 8 | import control.matlab as cont 9 | import matplotlib.pyplot as plt 10 | from scipy.integrate import odeint 11 | from scipy.integrate import solve_ivp 12 | 13 | import AStar as AS 14 | import Linear_Quadratic_Gaussian as lqg 15 | import Quad_Dynamics as QD 16 | 17 | def main(): 18 | 19 | # Path Planning 20 | show_animation = True 21 | 22 | print(__file__ + " start!!") 23 | 24 | # Start and goal position 25 | sx = 0.0 # [m] 26 | sy = 0.0 # [m] 27 | gx = 55.0 # [m] 28 | gy = 0.0 # [m] 29 | grid_size = 1.0 # [m] 30 | robot_radius = 4.0 # [m] 31 | 32 | # Set obstacle positions 33 | ox, oy = [], [] 34 | for i in range(-10, 60): # Bottom Wall 35 | ox.append(i) 36 | oy.append(-10.0) 37 | for i in range(-10, 60): # Right Wall 38 | ox.append(60.0) 39 | oy.append(i) 40 | for i in range(-10, 61): # Top Wall 41 | ox.append(i) 42 | oy.append(60.0) 43 | for i in range(-10, 61): # Left Wall 44 | ox.append(-10.0) 45 | oy.append(i) 46 | for i in range(-10, 40): # Mid Wall 1 47 | ox.append(10.0) 48 | oy.append(i) 49 | for i in range(0, 50): # Mid Wall 2 50 | ox.append(25.0) 51 | oy.append(60.0 - i) 52 | for i in range(-10, 50): # Mid Wall 3 53 | ox.append(40.0) 54 | oy.append(i) 55 | 56 | if show_animation: # pragma: no cover 57 | plt.plot(ox, oy, "ok") 58 | ax0 = plt.gca() 59 | start = plt.Circle((sx,sy),radius=1,color="green") 60 | goal = plt.Circle((gx,gy),radius=1,color="blue") 61 | ax0.add_patch(start) 62 | ax0.add_patch(goal) 63 | plt.grid(True) 64 | plt.axis("equal") 65 | ax0.set_xlabel('X-pos Meters(m)') 66 | ax0.set_ylabel('Y-pos Meters(m)') 67 | ax0.set_title('Floor Map') 68 | 69 | # Initialise A* Path Planning Class 70 | a_star = AS.AStarPlanner(ox, oy, grid_size, robot_radius) 71 | # A* Plan Path 72 | rx, ry = a_star.planning(sx, sy, gx, gy) 73 | 74 | if show_animation: # pragma: no cover 75 | plt.plot(rx, ry, "-y") 76 | plt.pause(0.001) 77 | #plt.show() 78 | 79 | # Import Control Config Files 80 | Adt = np.loadtxt("Adt.txt", delimiter=",") 81 | Bdt = np.loadtxt("Bdt.txt", delimiter=",") 82 | Cdt = np.loadtxt("Cdt.txt", delimiter=",") 83 | Ddt = np.loadtxt("Ddt.txt", delimiter=",") 84 | Kdt = np.loadtxt("Kdt.txt", delimiter=",") 85 | Kidt = np.loadtxt("Kidt.txt", delimiter=",") 86 | Ldt = np.loadtxt("Ldt.txt", delimiter=",") 87 | U_e = np.loadtxt("U_e.txt", delimiter=",") 88 | 89 | # Simulation Variables 90 | T = 0.01 91 | sFactor = 1 92 | Time = (len(rx) - 1)/sFactor 93 | kT = round(Time/T) 94 | t = np.arange(0.0,kT)*T 95 | st =(0,T) 96 | X = np.zeros((Adt.shape[0],1)) 97 | X0 = np.zeros(Adt.shape[0]) 98 | prevX = np.zeros((Adt.shape[0],1)) 99 | Xreal = np.zeros((Adt.shape[0],1)) 100 | dX = np.zeros((Adt.shape[0],1)) 101 | 102 | Ref = np.zeros((4,1)) 103 | Y = np.zeros((4,1)) 104 | U = np.zeros((Bdt.shape[1],1)) 105 | prevU = np.zeros((Bdt.shape[1],1)) 106 | 107 | Xplot = np.zeros((Adt.shape[0],kT)) 108 | Xrealplot = np.zeros((Adt.shape[0],kT)) 109 | Refplot = np.zeros((4,kT)) 110 | Yplot = np.zeros((4,kT)) 111 | Uplot = np.zeros((Bdt.shape[1],kT)) 112 | 113 | # Initialise LQG Class 114 | LQG = lqg.LQG(Adt,Bdt,Cdt,Ddt,Kdt,Kidt,Ldt,U_e) 115 | 116 | # Simulation 117 | for k in range(0,kT): 118 | 119 | # Track x and y path positions keep altitude to 1.5m 120 | Ref[0,0] = rx[len(rx)-int(k*sFactor/(T*10000))-1] 121 | Ref[1,0] = ry[len(ry)-int(k*sFactor/(T*10000))-1] 122 | Ref[2,0] = 1.5 123 | 124 | # Keep heading towards goal 125 | E_x = gx - X[0,0] 126 | E_y = gy - X[2,0] 127 | targPsi = (math.pi/2) - math.atan(E_y/E_x) 128 | Ref[3,0] = targPsi 129 | 130 | # Outputs 131 | Y = X[[0,2,4,10]] 132 | #Y = Xreal[[4,6,8,10]].reshape((4,1)) 133 | 134 | # Pass output and previous input into LQG function, return current input 135 | U = LQG.calculate(prevU,Y,Ref,True) 136 | 137 | # Linear Dynamics Simulation 138 | X = Adt @ X + Bdt @ U 139 | 140 | # Non Linear Dynamics Simulation 141 | #dX = QD.Quad_Dynamics(k,Xreal,U,1) # Forward Euler Integration Nonlinear Dynamics 142 | #Xreal = Xreal + T*dX.reshape((16,1)) 143 | 144 | # Non Linear Dynamics Simulation 145 | #Xy = solve_ivp(QD.Quad_Dynamics,st,X0,args=(Xreal.reshape(16),U.reshape(4)),method='RK45',vectorized=True) 146 | #Xreal = Xy.y[:,1] 147 | 148 | # Non Linear Dynamics Simulation 149 | #Xreal[:,0] = odeint(QD.Quad_Dynamics,X0,st,args=(Xreal[:,0],U[:,0]),tfirst=1)[1:] 150 | 151 | prevU = U 152 | 153 | Refplot[:,k] = Ref[:,0] 154 | Uplot[:,k] = U[:,0] 155 | 156 | # Linear States and Outputs 157 | Yplot[:,k] = Y[:,0] 158 | Xplot[:,k] = X[:,0] 159 | 160 | # Non Linear States and Outputs 161 | #Yplot[:,k] = Y.reshape(4) 162 | #Xplot[:,k] = Xreal.reshape(16) 163 | #print(Xplot[0,k]) 164 | 165 | # Plots 166 | fig, ax = plt.subplots() 167 | ax.plot(t,Xplot[0,:]) 168 | ax.plot(t,Refplot[0,:],"-g") 169 | ax.plot(t,Xplot[2,:]) 170 | ax.plot(t,Refplot[1,:],"-b") 171 | ax.plot(t,Xplot[4,:]) 172 | ax.plot(t,Refplot[2,:],"-r") 173 | ax.grid(True) 174 | ax.set_xlabel('Time(s)') 175 | ax.set_ylabel('Meters(m)') 176 | ax.set_title('Position') 177 | 178 | fig2, ax2 = plt.subplots() 179 | ax2.plot(t,Xplot[6,:]*180/math.pi,"-g") 180 | ax2.plot(t,Xplot[8,:]*180/math.pi,"-b") 181 | ax2.plot(t,Xplot[10,:]*180/math.pi,"-r") 182 | ax2.plot(t,Refplot[3,:]*180/math.pi) 183 | ax2.grid(True) 184 | ax2.set_xlabel('Time(s)') 185 | ax2.set_ylabel('Degrees(d)') 186 | ax2.set_title('Attitude') 187 | 188 | fig3, ax3 = plt.subplots() 189 | ax3.plot(t,Uplot[0,:]) 190 | ax3.plot(t,Uplot[1,:]) 191 | ax3.plot(t,Uplot[2,:]) 192 | ax3.plot(t,Uplot[3,:]) 193 | ax3.grid(True) 194 | ax3.set_xlabel('Time(s)') 195 | ax3.set_ylabel('Micro Seconds(ms)') 196 | ax3.set_title('Inputs PWM Signal') 197 | 198 | # Add UAV path to floor plan plot 199 | for k in range(0,kT): 200 | UAV = plt.Circle((Xplot[0,k],Xplot[2,k]),radius=1,color="red") 201 | ax0.add_patch(UAV) 202 | ''' 203 | rectangle = plt.Rectangle((10,10),width=2,height=4,color="red") 204 | ''' 205 | plt.show() 206 | 207 | if __name__ == '__main__': 208 | main() 209 | 210 | -------------------------------------------------------------------------------- /SLAM/EKF-SLAM.py: -------------------------------------------------------------------------------- 1 | """ 2 | Extended Kalman Filter SLAM example 3 | author: Atsushi Sakai (@Atsushi_twi) 4 | """ 5 | 6 | import math 7 | 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | 11 | # EKF state covariance 12 | Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2 13 | 14 | # Simulation parameter 15 | Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2 16 | R_sim = np.diag([1.0, np.deg2rad(10.0)]) ** 2 17 | 18 | DT = 0.1 # time tick [s] 19 | SIM_TIME = 50.0 # simulation time [s] 20 | MAX_RANGE = 20.0 # maximum observation range 21 | M_DIST_TH = 2.0 # Threshold of Mahalanobis distance for data association. 22 | STATE_SIZE = 3 # State size [x,y,yaw] 23 | LM_SIZE = 2 # LM state size [x,y] 24 | 25 | show_animation = True 26 | 27 | 28 | def ekf_slam(xEst, PEst, u, z): 29 | # Predict 30 | S = STATE_SIZE 31 | G, Fx = jacob_motion(xEst[0:S], u) 32 | xEst[0:S] = motion_model(xEst[0:S], u) 33 | PEst[0:S, 0:S] = G.T @ PEst[0:S, 0:S] @ G + Fx.T @ Cx @ Fx 34 | initP = np.eye(2) 35 | 36 | # Update 37 | for iz in range(len(z[:, 0])): # for each observation 38 | min_id = search_correspond_landmark_id(xEst, PEst, z[iz, 0:2]) 39 | 40 | nLM = calc_n_lm(xEst) 41 | if min_id == nLM: 42 | print("New LM") 43 | # Extend state and covariance matrix 44 | xAug = np.vstack((xEst, calc_landmark_position(xEst, z[iz, :]))) 45 | PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))), 46 | np.hstack((np.zeros((LM_SIZE, len(xEst))), initP)))) 47 | xEst = xAug 48 | PEst = PAug 49 | lm = get_landmark_position_from_state(xEst, min_id) 50 | y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], min_id) 51 | 52 | K = (PEst @ H.T) @ np.linalg.inv(S) 53 | xEst = xEst + (K @ y) 54 | PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst 55 | 56 | xEst[2] = pi_2_pi(xEst[2]) 57 | 58 | return xEst, PEst 59 | 60 | 61 | def calc_input(): 62 | v = 1.0 # [m/s] 63 | yaw_rate = 0.1 # [rad/s] 64 | u = np.array([[v, yaw_rate]]).T 65 | return u 66 | 67 | 68 | def observation(xTrue, xd, u, RFID): 69 | xTrue = motion_model(xTrue, u) 70 | 71 | # add noise to gps x-y 72 | z = np.zeros((0, 3)) 73 | 74 | for i in range(len(RFID[:, 0])): 75 | 76 | dx = RFID[i, 0] - xTrue[0, 0] 77 | dy = RFID[i, 1] - xTrue[1, 0] 78 | d = math.hypot(dx, dy) 79 | angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0]) 80 | if d <= MAX_RANGE: 81 | dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise 82 | angle_n = angle + np.random.randn() * Q_sim[1, 1] ** 0.5 # add noise 83 | zi = np.array([dn, angle_n, i]) 84 | z = np.vstack((z, zi)) 85 | 86 | # add noise to input 87 | ud = np.array([[ 88 | u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5, 89 | u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5]]).T 90 | 91 | xd = motion_model(xd, ud) 92 | return xTrue, z, xd, ud 93 | 94 | 95 | def motion_model(x, u): 96 | F = np.array([[1.0, 0, 0], 97 | [0, 1.0, 0], 98 | [0, 0, 1.0]]) 99 | 100 | B = np.array([[DT * math.cos(x[2, 0]), 0], 101 | [DT * math.sin(x[2, 0]), 0], 102 | [0.0, DT]]) 103 | 104 | x = (F @ x) + (B @ u) 105 | return x 106 | 107 | 108 | def calc_n_lm(x): 109 | n = int((len(x) - STATE_SIZE) / LM_SIZE) 110 | return n 111 | 112 | 113 | def jacob_motion(x, u): 114 | Fx = np.hstack((np.eye(STATE_SIZE), np.zeros( 115 | (STATE_SIZE, LM_SIZE * calc_n_lm(x))))) 116 | 117 | jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])], 118 | [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])], 119 | [0.0, 0.0, 0.0]], dtype=np.float64) 120 | 121 | G = np.eye(STATE_SIZE) + Fx.T @ jF @ Fx 122 | 123 | return G, Fx, 124 | 125 | 126 | def calc_landmark_position(x, z): 127 | zp = np.zeros((2, 1)) 128 | 129 | zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1]) 130 | zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1]) 131 | 132 | return zp 133 | 134 | 135 | def get_landmark_position_from_state(x, ind): 136 | lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :] 137 | 138 | return lm 139 | 140 | 141 | def search_correspond_landmark_id(xAug, PAug, zi): 142 | """ 143 | Landmark association with Mahalanobis distance 144 | """ 145 | 146 | nLM = calc_n_lm(xAug) 147 | 148 | min_dist = [] 149 | 150 | for i in range(nLM): 151 | lm = get_landmark_position_from_state(xAug, i) 152 | y, S, H = calc_innovation(lm, xAug, PAug, zi, i) 153 | min_dist.append(y.T @ np.linalg.inv(S) @ y) 154 | 155 | min_dist.append(M_DIST_TH) # new landmark 156 | 157 | min_id = min_dist.index(min(min_dist)) 158 | 159 | return min_id 160 | 161 | 162 | def calc_innovation(lm, xEst, PEst, z, LMid): 163 | delta = lm - xEst[0:2] 164 | q = (delta.T @ delta)[0, 0] 165 | z_angle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0] 166 | zp = np.array([[math.sqrt(q), pi_2_pi(z_angle)]]) 167 | y = (z - zp).T 168 | y[1] = pi_2_pi(y[1]) 169 | H = jacob_h(q, delta, xEst, LMid + 1) 170 | S = H @ PEst @ H.T + Cx[0:2, 0:2] 171 | 172 | return y, S, H 173 | 174 | 175 | def jacob_h(q, delta, x, i): 176 | sq = math.sqrt(q) 177 | G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]], 178 | [delta[1, 0], - delta[0, 0], - q, - delta[1, 0], delta[0, 0]]]) 179 | 180 | G = G / q 181 | nLM = calc_n_lm(x) 182 | F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM)))) 183 | F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))), 184 | np.eye(2), np.zeros((2, 2 * nLM - 2 * i)))) 185 | 186 | F = np.vstack((F1, F2)) 187 | 188 | H = G @ F 189 | 190 | return H 191 | 192 | 193 | def pi_2_pi(angle): 194 | return (angle + math.pi) % (2 * math.pi) - math.pi 195 | 196 | 197 | def main(): 198 | print(__file__ + " start!!") 199 | 200 | time = 0.0 201 | 202 | # RFID positions [x, y] 203 | RFID = np.array([[10.0, -2.0], 204 | [15.0, 10.0], 205 | [3.0, 15.0], 206 | [-5.0, 20.0]]) 207 | 208 | # State Vector [x y yaw v]' 209 | xEst = np.zeros((STATE_SIZE, 1)) 210 | xTrue = np.zeros((STATE_SIZE, 1)) 211 | PEst = np.eye(STATE_SIZE) 212 | 213 | xDR = np.zeros((STATE_SIZE, 1)) # Dead reckoning 214 | 215 | # history 216 | hxEst = xEst 217 | hxTrue = xTrue 218 | hxDR = xTrue 219 | 220 | while SIM_TIME >= time: 221 | time += DT 222 | u = calc_input() 223 | 224 | xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID) 225 | 226 | xEst, PEst = ekf_slam(xEst, PEst, ud, z) 227 | 228 | x_state = xEst[0:STATE_SIZE] 229 | 230 | # store data history 231 | hxEst = np.hstack((hxEst, x_state)) 232 | hxDR = np.hstack((hxDR, xDR)) 233 | hxTrue = np.hstack((hxTrue, xTrue)) 234 | 235 | if show_animation: # pragma: no cover 236 | plt.cla() 237 | # for stopping simulation with the esc key. 238 | plt.gcf().canvas.mpl_connect( 239 | 'key_release_event', 240 | lambda event: [exit(0) if event.key == 'escape' else None]) 241 | 242 | plt.plot(RFID[:, 0], RFID[:, 1], "*k") 243 | plt.plot(xEst[0], xEst[1], ".r") 244 | 245 | # plot landmark 246 | for i in range(calc_n_lm(xEst)): 247 | plt.plot(xEst[STATE_SIZE + i * 2], 248 | xEst[STATE_SIZE + i * 2 + 1], "xg") 249 | 250 | plt.plot(hxTrue[0, :], 251 | hxTrue[1, :], "-b") 252 | plt.plot(hxDR[0, :], 253 | hxDR[1, :], "-k") 254 | plt.plot(hxEst[0, :], 255 | hxEst[1, :], "-r") 256 | plt.axis("equal") 257 | plt.grid(True) 258 | plt.pause(0.001) 259 | 260 | 261 | if __name__ == '__main__': 262 | main() 263 | -------------------------------------------------------------------------------- /X4 LQG Controller/12States/ControlDesignLQG12States.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; % keeps numerical output SF low 5 | 6 | %% Octave Packeges 7 | pkg load control; 8 | 9 | %% Mass of the Multirotor in Kilograms as taken from the CAD 10 | M = 1.157685; 11 | g = 9.81; 12 | 13 | %% Dimensions of Multirotor 14 | L = 0.16319; % along X-axis and Y-axis Distance from left and right motor pair to center of mass 15 | 16 | %% Mass Moment of Inertia as Taken from the CAD 17 | Ixx = 1.129E+05/(1000*10000); 18 | Iyy = 1.129E+05/(1000*10000); 19 | Izz = 2.033E+05/(1000*10000); 20 | 21 | %% Motor Thrust and Torque Constants (To be determined experimentally) 22 | Ktau = 7.708e-10 * 2; 23 | Kthrust = 1.812e-07; 24 | Kthrust2 = 0.0007326; 25 | Mtau = (1/44.22); 26 | Ku = 515.5; 27 | 28 | %% Equilibrium Input 29 | W_e = ((-4*Kthrust2) + sqrt((4*Kthrust2)^2 - (4*(-M*g)*(4*Kthrust))))/(2*(4*Kthrust))*ones(4,1); 30 | U_e = (W_e/(Ku*Mtau)); 31 | 32 | %% Define Discrete-Time computer Dynamics 33 | T = 0.01; % Sample period (s)- 100Hz 34 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 35 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 36 | 37 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 38 | 39 | % A = 12x12 matrix 40 | A = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 41 | 0, 0, 0, 0, 0, 0, 0, 0, -2*Kthrust*W_e(1)/M, -2*Kthrust*W_e(2)/M, -2*Kthrust*W_e(3)/M, -2*Kthrust*W_e(4)/M; 42 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0; 43 | 0, 0, 0, 0, 0, 0, 0, 0, L*2*Kthrust*W_e(1)/Ixx, L*2*Kthrust*W_e(2)/Ixx, -L*2*Kthrust*W_e(3)/Ixx, -L*2*Kthrust*W_e(4)/Ixx; 44 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0; 45 | 0, 0, 0, 0, 0, 0, 0, 0, L*2*Kthrust*W_e(1)/Iyy, -L*2*Kthrust*W_e(2)/Iyy, L*2*Kthrust*W_e(3)/Iyy, -L*2*Kthrust*W_e(4)/Iyy; 46 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; 47 | 0, 0, 0, 0, 0, 0, 0, 0, 2*Ktau*W_e(1)/Izz, -2*Ktau*W_e(2)/Izz, -2*Ktau*W_e(3)/Izz, 2*Ktau*W_e(4)/Izz; 48 | 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0; 49 | 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0; 50 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0; 51 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau]; 52 | 53 | % B = 12x4 matrix 54 | B = [0, 0, 0, 0; 55 | 0, 0, 0, 0; 56 | 0, 0, 0, 0; 57 | 0, 0, 0, 0; 58 | 0, 0, 0, 0; 59 | 0, 0, 0, 0; 60 | 0, 0, 0, 0; 61 | 0, 0, 0, 0; 62 | Ku, 0, 0, 0; 63 | 0, Ku, 0, 0; 64 | 0, 0, Ku, 0; 65 | 0, 0, 0, Ku]; 66 | 67 | % C = 4x12 matrix 68 | C = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 69 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 70 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0; 71 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 72 | 73 | % D = 4x4 matrix 74 | D = [0, 0, 0, 0; 75 | 0, 0, 0, 0; 76 | 0, 0, 0, 0; 77 | 0, 0, 0, 0]; 78 | 79 | %% Discrete-Time System 80 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 81 | 82 | Adt = sysdt.a; 83 | Bdt = sysdt.b; 84 | Cdt = sysdt.c; 85 | Ddt = sysdt.d; 86 | 87 | %% System Characteristics 88 | poles = eig(Adt); 89 | % System is maginally Unstable 90 | 91 | figure(1) 92 | plot(poles,'*') 93 | grid on 94 | title('Discrete System Eigenvalues') 95 | 96 | cntr = rank(ctrb(Adt,Bdt)); 97 | % Fully Reachable 98 | 99 | obs = rank(obsv(Adt,Cdt)); 100 | % Fully Observable 101 | 102 | %% Discrete-Time Full Integral Augmaneted System 103 | Cr = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 104 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 105 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0; 106 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 107 | 108 | r = 4; % number of reference inputs 109 | n = size(A,2); % number of states 110 | q = size(Cr,1); % number of controlled outputs 111 | 112 | Dr = zeros(q,4); 113 | 114 | Adtaug = [Adt zeros(n,r); 115 | -Cr*Adt eye(q,r)]; 116 | 117 | Bdtaug = [Bdt; 118 | -Cr*Bdt]; 119 | 120 | Cdtaug = [C zeros(r,r)]; 121 | 122 | %% Discrete-Time Full State-Feedback Control 123 | % State feedback control design with integral control via state augmentation 124 | % Z Phi Theta Psi are controlled outputs 125 | Q = diag([8000,0,2000,0,2000,0,8000,0,0,0,0,0,7,25,25,0.5]); % State penalty 126 | R = eye(4,4)*(10^-4); % Control penalty 127 | 128 | Kdtaug = dlqr(Adtaug,Bdtaug,Q,R); % DT State-Feedback Controller Gains 129 | Kdt = Kdtaug(:,1:n); % LQR Gains 130 | Kidt = Kdtaug(:,n+1:end); % Integral Gains 131 | 132 | %% Discrete-Time Kalman Filter Design x(k+1) = Adt*x + Bdt*u + Gdt*w, y = Cdt*x + Ddt*u + Hdt*w + v 133 | %Hdt = zeros(size(Cy,1),size(Gdt,2)); % No process noise on measurements 134 | 135 | Gdt = 1e-1*eye(n); 136 | 137 | Rw = diag([0.1,1,1,0.1,1,0.1,1,0.1,1000,1000,1000,1000]); % Process noise covariance matrix 138 | Rv = diag([10^-5,10^-5,10^-5,10^-5]); % Measurement noise covariance matrix (from sensor data) 139 | 140 | Ldt = dlqe(Adt,Gdt,Cr,Rw,Rv); 141 | 142 | %% Dynamic Simulation 143 | Time = 50; 144 | kT = round(Time/T); % Simulation steps 145 | 146 | Xreal = zeros(16,kT); % Non-linear states 147 | X = zeros(12,kT); % Linear states 148 | Xest = X; % Estimated states 149 | Y = zeros(4,kT); % Controlled output states 150 | e = zeros(4,kT); % Estimation error 151 | Xe = zeros(4,kT); % Integral states 152 | 153 | U = ones(4,kT); % System input/controller output 154 | U(:,1) = U_e; 155 | 156 | Ref = [0;0;0;0]; % Reference vector 157 | 158 | t_span = [0,T]; 159 | 160 | for k = 2:kT-1 161 | 162 | % Step reference setting 163 | if k == 10/T 164 | Ref(1) = -1; 165 | end 166 | if k == 15/T 167 | Ref(2) = 30*pi/180; 168 | end 169 | if k == 20/T 170 | Ref(2) = 0; 171 | end 172 | if k == 25/T 173 | Ref(3) = 30*pi/180; 174 | end 175 | if k == 30/T 176 | Ref(3) = 0; 177 | end 178 | if k == 40/T 179 | Ref(4) = 90*pi/180; 180 | end 181 | 182 | %Estimation 183 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % No KF Linear Prediction 184 | % Xest(:,k) = Xreal([5,6,7,8,9,10,11,12,13:16],k); % No KF Non Linear Prediction 185 | 186 | % Y(:,k) = Xreal([5,7,9,11],k); 187 | % xkf = [0;0;0;0;Xest(:,k-1)]; 188 | % xode = ode45(@(t,X) Quad_Dynamics(t,X,U(:,k-1)),t_span,xkf); % Nonlinear Prediction 189 | % Xest(:,k) = xode.y(5:16,end); 190 | % e(:,k) = [Y(:,k) - Xest([1,3,5,7],k)]; 191 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 192 | 193 | Y(:,k) = Xreal([5,7,9,11],k); 194 | Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Linear Prediction 195 | e(:,k) = [Y(:,k) - Xest([1,3,5,7],k)]; 196 | Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 197 | 198 | %Control 199 | Xe(:,k) = Xe(:,k-1) + (Ref - Xest([1,3,5,7],k)); % Integrator 200 | U(:,k) = min(800, max(0, Ref + U_e - [Kdt,Kidt]*[Xest(:,k);Xe(:,k)])); % Constraint Saturation 201 | 202 | %Simulation 203 | t_span = [0,T]; 204 | xode = ode45(@(t,X) Quad_Dynamics_V(t,X,U(:,k)),t_span,Xreal(:,k)); % Runge-Kutta Integration Nonlinear Dynamics 205 | Xreal(:,k+1) = xode.y(:,end); 206 | 207 | % X(:,k+1) = Adt*X(:,k) + Bdt*U(:,k); % Fully Linear Dynamics 208 | end 209 | 210 | PROT = profile("info"); 211 | profile off; 212 | 213 | Rad2Deg = [180/pi,180/pi,180/pi]'; % Convert radians to degrees 214 | 215 | %% Plots 216 | t = (0:kT-1)*T; 217 | figure(2) 218 | subplot(2,1,1) 219 | plot(t,Xreal(5,:)*-1) 220 | legend('Alt') 221 | title('Real Altitude') 222 | xlabel('Time(s)') 223 | ylabel('Meters(m)') 224 | 225 | subplot(2,1,2) 226 | plot(t,Xreal([7,9,11],:).*Rad2Deg) 227 | legend('\phi','\theta','\psi') 228 | title('Real Attitude') 229 | xlabel('Time(s)') 230 | ylabel('Degrees(d)') 231 | 232 | figure(3) 233 | subplot(2,1,1) 234 | plot(t,Xest(1,:)*-1) 235 | legend('Alt_e') 236 | title('Estimated Altitude') 237 | xlabel('Time(s)') 238 | ylabel('Meters(m)') 239 | 240 | subplot(2,1,2) 241 | plot(t,Xest([3,5,7],:).*Rad2Deg) 242 | legend('\phi_e','\theta_e','\psi_e') 243 | title('Estimated Attitude') 244 | xlabel('Time(s)') 245 | ylabel('Degrees(d)') 246 | 247 | figure(4) 248 | subplot(2,1,1) 249 | plot(t,e(1,:)) 250 | legend('e_z') 251 | title('Altitude prediction error') 252 | xlabel('Time(s)') 253 | ylabel('Error meters(m)') 254 | 255 | subplot(2,1,2) 256 | plot(t,e([2,3,4],:).*Rad2Deg) 257 | legend('e_\phi','e_\theta','e_\psi') 258 | title('Attitude prediction error') 259 | xlabel('Time(s)') 260 | ylabel('Error degrees(d)') 261 | 262 | figure(5) 263 | plot(t,U) 264 | legend('U1','U2','U3','U4') 265 | title('Inputs PWM Signal') 266 | xlabel('Time(s)') 267 | ylabel('Micro Seconds(ms)') 268 | 269 | 270 | Cpoles = eig(Adtaug - (Bdtaug*[Kdt,Kidt])); 271 | % System Unstable 272 | 273 | figure(6) 274 | plot(Cpoles(1:14),'*') 275 | grid on 276 | title('Closed-Loop Eigenvalues') 277 | 278 | 279 | %% PRINT TO CONFIGURATION FILES 280 | dlmwrite ("Adt.txt", single(Adt),',', 0, 0) 281 | 282 | dlmwrite ("Bdt.txt", single(Bdt),',', 0, 0) 283 | 284 | dlmwrite ("Cdt.txt", single(Cdt),',', 0, 0) 285 | 286 | dlmwrite ("Ddt.txt", single(Ddt),',', 0, 0) 287 | 288 | dlmwrite ("Kdt.txt", single(Kdt),',', 0, 0) 289 | 290 | dlmwrite ("Kidt.txt", single(Kidt),',', 0, 0) 291 | 292 | dlmwrite ("Ldt.txt", single(Ldt),',', 0, 0) 293 | 294 | dlmwrite ("U_e.txt", single(U_e),',', 0, 0) 295 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/ControlDesignLQG16StatesJacobian.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Octave Packeges 7 | pkg load control; 8 | pkg load optim; 9 | pkg load symbolic; 10 | 11 | warning ("off"); 12 | 13 | %% Mass of the Multirotor in Kilograms as taken from the CAD 14 | 15 | M = 0.857945; 16 | g = 9.81; 17 | 18 | %% Dimensions of Multirotor 19 | 20 | L = 0.16319; % along X-axis and Y-axis Distance from left and right motor pair to center of mass 21 | 22 | %% Mass Moment of Inertia as Taken from the CAD 23 | 24 | Ixx = 1.061E+05/(1000*10000); 25 | Iyy = 1.061E+05/(1000*10000); 26 | Izz = 2.011E+05/(1000*10000); 27 | 28 | %% Motor Thrust and Torque Constants (To be determined experimentally) 29 | 30 | Ktau = 7.708e-10 * 2; 31 | Kthrust = 1.812e-07; 32 | Kthrust2 = 0.0007326; 33 | Mtau = 1/44.22; 34 | Ku = 515.5; 35 | 36 | %% Jacobian Linearisation 37 | syms x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12 x13 x14 x15 x16 38 | syms u1 u2 u3 u4 39 | 40 | Xs = [x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12 x13 x14 x15 x16].'; 41 | Us = [u1 u2 u3 u4].'; 42 | 43 | dX = Quad_Dynamics(0,Xs,Us); 44 | 45 | %% Equilibrium Points 46 | 47 | W_e = ((-4*Kthrust2) + sqrt((4*Kthrust2)^2 - (4*(-M*g)*(4*Kthrust))))/(2*(4*Kthrust))*ones(4,1); 48 | U_e = (W_e/(Ku*Mtau)); 49 | X_e = [x1;0;x2;0;x3;0;0;0;0;0;0;0;W_e]; 50 | 51 | %% Jacobian Matrices 52 | 53 | JA = jacobian(dX,Xs.'); 54 | JB = jacobian(dX,Us.'); 55 | 56 | %% Define Discrete-Time BeagleBone Dynamics 57 | 58 | T = 0.01; % Sample period (s)- 100Hz 59 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 60 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 61 | 62 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 63 | 64 | JA1 = subs(JA,Xs,X_e); 65 | A = subs(JA1,Us,U_e); 66 | A = eval(A); 67 | 68 | JB1 = subs(JB,Xs,X_e); 69 | B = subs(JB1,Us,U_e); 70 | B = eval(B); 71 | 72 | % C = 4x16 matrix 73 | C = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 74 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 75 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 76 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 77 | 78 | % D = 4x4 matrix 79 | D = [0, 0, 0, 0; 80 | 0, 0, 0, 0; 81 | 0, 0, 0, 0; 82 | 0, 0, 0, 0]; 83 | 84 | %% Discrete-Time System 85 | 86 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 87 | 88 | Adt = sysdt.a; 89 | Bdt = sysdt.b; 90 | Cdt = sysdt.c; 91 | Ddt = sysdt.d; 92 | 93 | %% System Characteristics 94 | 95 | poles = eig(Adt); 96 | % System Unstable 97 | 98 | figure(1); 99 | plot(poles,'*'); 100 | grid on; 101 | title('Discrete System Eigenvalues') 102 | 103 | cntr = rank(ctrb(Adt,Bdt)) 104 | % Fully Reachable 105 | 106 | obs = rank(obsv(Adt,Cdt)) 107 | % Fully Observable 108 | 109 | 110 | %% Discrete-Time Full Integral Augmaneted System 111 | 112 | Cr = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 113 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 114 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 115 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 116 | 117 | u = size(B,2); % number of controlled inputs 118 | r = size(Cr,1); % number of reference inputs 119 | n = size(A,2); % number of states 120 | q = size(Cr,1); % number of controlled outputs 121 | 122 | Dr = zeros(q,u); 123 | 124 | Adtaug = [Adt zeros(n,r); 125 | -Cr*Adt eye(q,r)]; 126 | 127 | Bdtaug = [Bdt; 128 | -Cr*Bdt]; 129 | 130 | Cdtaug = [C zeros(r,r)]; 131 | 132 | %% Discrete-Time Full State-Feedback Control 133 | % State feedback control design with integral control via state augmentation 134 | % X Y Z Psi are controlled outputs 135 | 136 | Q = diag([5000,0,5000,0,5000,0,1000,0,1000,0,5000,0,0,0,0,0,5,5,5,0.4]); % State penalty 137 | %Q = diag([5,0,5,0,5,0,10,0,10,0,5,0,0,0,0,0,1,1,1,0.1]); % State penalty 138 | R = eye(4,4)*(10^-4); % Control penalty 139 | 140 | Kdtaug = dlqr(Adtaug,Bdtaug,Q,R); % DT State-Feedback Controller Gains 141 | Kdt = Kdtaug(:,1:n); % LQR Gains 142 | Kidt = Kdtaug(:,n+1:end); % Integral Gains 143 | 144 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 145 | 146 | Gdt = 1e-1*eye(n); 147 | 148 | Rw = diag([0.1,1,0.1,1,0.1,1,1,0.1,1,0.1,1,0.1,1000,1000,1000,1000]); % Process noise covariance matrix 149 | Rv = diag([10^-5,10^-5,10^-5,10^-5]); % Measurement noise covariance matrix Note: use low gausian noice for Rv 150 | 151 | Ldt = dlqe(Adt,Gdt,Cdt,Rw,Rv); 152 | 153 | %Hdt = zeros(size(Cy,1),size(Gdt,2)); % No process noise on measurements 154 | %sys4kf = ss(Adt,[Bdt Gdt],Cy,[Dy Hdt],T); 155 | %sys4kf = ss(Adt,Bdt,Cy,Dy,T); 156 | %[kalm,Ldt] = kalman(sys4kf,Rw,Rv); 157 | 158 | %% Dynamic Simulation 159 | 160 | Time = 60; 161 | kT = round(Time/T); 162 | 163 | X = zeros(16,kT); 164 | Xreal = zeros(16,kT); 165 | Xest = zeros(16,kT); 166 | Xe = zeros(4,kT); 167 | Y = zeros(4,kT); 168 | e = zeros(4,kT); 169 | 170 | U = ones(4,kT); 171 | U(:,1) = U_e; 172 | U(:,2) = U_e; 173 | Ref = [0;0;0;0]; 174 | 175 | t_span = [0,T]; 176 | 177 | for k = 3:kT-1 178 | 179 | %% Reference Setting 180 | 181 | if k == 10/T 182 | Ref(1) = 1; 183 | end 184 | if k == 15/T 185 | Ref(1) = 0; 186 | end 187 | if k == 20/T 188 | Ref(2) = 1; 189 | end 190 | if k == 25/T 191 | Ref(2) = 0; 192 | end 193 | if k == 30/T 194 | Ref(3) = 1; 195 | end 196 | if k == 35/T 197 | Ref(3) = 0; 198 | end 199 | if k == 40/T 200 | Ref(4) = 90*pi/180; 201 | end 202 | if k == 50/T 203 | Ref(4) = 0; 204 | end 205 | 206 | %% Estimation 207 | 208 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % No KF Linear Prediction 209 | 210 | % Xest(:,k) = Xreal(:,k); % No KF Non Linear Prediction 211 | 212 | % Y(:,k) = Xreal([1,3,5,11],k); 213 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Linear Kalman Prediction 214 | % e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 215 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 216 | 217 | % Y(:,k) = Xreal([1,3,5,11],k); 218 | % xkf = [Xest(:,k-1)]; 219 | % xode = ode45(@(t,X) Quad_Dynamics(t,X,U(:,k-1)),t_span,xkf); % Limited Nonlinear Kalman Prediction 220 | % Xest(:,k) = xode.y(:,end); 221 | % e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 222 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 223 | 224 | JA1 = subs(JA,Xs,Xest(:,k)); 225 | JA2 = subs(JA1,Us,U(:,k)); 226 | A = eval(JA2); 227 | JB1 = subs(JB,Xs,Xest(:,k)); 228 | JB2 = subs(JB1,Us,U(:,k)); 229 | B = eval(JB2); 230 | 231 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Extended Kalman Prediction 232 | Adt = sysdt.a; 233 | 234 | LdtEkf = dlqe(Adt,Gdt,Cdt,Rw,Rv); 235 | 236 | Y(:,k) = Xreal([1,3,5,11],k); 237 | Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); 238 | e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 239 | Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 240 | 241 | %% Control 242 | 243 | Xe(:,k) = Xe(:,k-1) + (Ref - Xest([1,3,5,11],k)); % Integrator 244 | U(:,k) = min(800, max(0, U_e - [Kdt,Kidt]*[Xest(:,k) ;Xe(:,k)])); % Constraint Saturation 245 | 246 | %% Simulation 247 | %{ 248 | t_span = [0,T]; 249 | xode = ode45(@(t,X) Quad_Dynamics(t,X,U(:,k)),t_span,Xreal(:,k)); % Runge-Kutta Integration Nonlinear Dynamics 250 | Xreal(:,k+1) = xode.y(:,end); 251 | %} 252 | % t=t_span; 253 | % [dX] = Quad_Dynamics(t,Xreal(:,k),U(:,k)); % Forward Euler Integration Nonlinear Dynamics 254 | % Xreal(:,k+1) = Xreal(:,k) + T*dX; 255 | 256 | Xreal(:,k+1) = Adt*Xreal(:,k) + Bdt*(U(:,k)-U_e); % Fully Linear Dynamics 257 | end 258 | 259 | %PROT = profile("info"); 260 | %profile off; 261 | 262 | Rad2Deg = [180/pi,180/pi,180/pi]'; 263 | 264 | %Plots 265 | t = (0:kT-1)*T; 266 | figure(2); 267 | subplot(2,1,1); 268 | plot(t,Xreal([1,3,5],:)); 269 | legend('X','Y','Z'); 270 | title('Real Position'); 271 | xlabel('Time(s)'); 272 | ylabel('Meters(m)'); 273 | 274 | subplot(2,1,2); 275 | plot(t,Xreal([7,9,11],:).*Rad2Deg); 276 | legend('\phi','\theta','\psi'); 277 | title('Real Attitude'); 278 | xlabel('Time(s)'); 279 | ylabel('Degrees(d)'); 280 | 281 | figure(3); 282 | subplot(2,1,1); 283 | plot(t,Xest([1,3,5],:)); 284 | legend('X_e','Y_e','Z_e'); 285 | title('Estimated Position'); 286 | xlabel('Time(s)'); 287 | ylabel('Meters(m)'); 288 | 289 | subplot(2,1,2); 290 | plot(t,Xest([7,9,11],:).*Rad2Deg); 291 | legend('\phi_e','\theta_e','\psi_e'); 292 | title('Estimated Attitude'); 293 | xlabel('Time(s)'); 294 | ylabel('Degrees(d)'); 295 | 296 | figure(4); 297 | subplot(2,1,1); 298 | plot(t,e([1,2,3],:)); 299 | legend('e_x','e_y','e_z'); 300 | title('Position prediction error'); 301 | xlabel('Time(s)'); 302 | ylabel('Error meters(m)'); 303 | 304 | subplot(2,1,2); 305 | plot(t,e(4,:)*Rad2Deg(1)); 306 | legend('e_\psi'); 307 | title('Hedding prediction error'); 308 | xlabel('Time(s)'); 309 | ylabel('Error degrees(d)'); 310 | 311 | figure(5); 312 | plot(t,U); 313 | legend('U1','U2','U3','U4'); 314 | title('Inputs PWM Signal'); 315 | xlabel('Time(s)'); 316 | ylabel('Micro Seconds(ms)'); 317 | 318 | Cpoles = eig(Adtaug - (Bdtaug*[Kdt,Kidt])); 319 | figure(6); 320 | plot(Cpoles,'*'); 321 | grid on; 322 | title('Closed-Loop Eigenvalues'); 323 | 324 | Obspoles = eig(Adt - (Ldt*Cdt)); 325 | figure(7); 326 | plot(Obspoles,'*'); 327 | grid on; 328 | title('Closed-Loop Estimator Eigenvalues'); 329 | 330 | %% PRINT TO CONFIGURATION FILES 331 | %{ 332 | dlmwrite("Adt.txt", Adt,',', 0, 0); 333 | dlmwrite("Bdt.txt", Bdt,',', 0, 0); 334 | dlmwrite("Cdt.txt", Cdt,',', 0, 0); 335 | dlmwrite("Ddt.txt", Ddt,',', 0, 0); 336 | dlmwrite("Kdt.txt", Kdt,',', 0, 0); 337 | dlmwrite("Kidt.txt", Kidt,',', 0, 0); 338 | dlmwrite("Ldt.txt", Ldt,',', 0, 0); 339 | dlmwrite("U_e.txt", U_e,',', 0, 0); 340 | %} -------------------------------------------------------------------------------- /Planning/AStar.py: -------------------------------------------------------------------------------- 1 | """ 2 | A* grid planning 3 | author: Atsushi Sakai(@Atsushi_twi) 4 | Nikos Kanargias (nkana@tee.gr) 5 | See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) 6 | """ 7 | 8 | import math 9 | 10 | import matplotlib.pyplot as plt 11 | 12 | show_animation = True 13 | 14 | 15 | class AStarPlanner: 16 | 17 | def __init__(self, ox, oy, resolution, rr): 18 | """ 19 | Initialize grid map for a star planning 20 | ox: x position list of Obstacles [m] 21 | oy: y position list of Obstacles [m] 22 | resolution: grid resolution [m] 23 | rr: robot radius[m] 24 | """ 25 | 26 | self.resolution = resolution 27 | self.rr = rr 28 | self.min_x, self.min_y = 0, 0 29 | self.max_x, self.max_y = 0, 0 30 | self.obstacle_map = None 31 | self.x_width, self.y_width = 0, 0 32 | self.motion = self.get_motion_model() 33 | self.calc_obstacle_map(ox, oy) 34 | 35 | class Node: 36 | def __init__(self, x, y, cost, parent_index): 37 | self.x = x # index of grid 38 | self.y = y # index of grid 39 | self.cost = cost 40 | self.parent_index = parent_index 41 | 42 | def __str__(self): 43 | return str(self.x) + "," + str(self.y) + "," + str( 44 | self.cost) + "," + str(self.parent_index) 45 | 46 | def planning(self, sx, sy, gx, gy): 47 | """ 48 | A star path search 49 | input: 50 | s_x: start x position [m] 51 | s_y: start y position [m] 52 | gx: goal x position [m] 53 | gy: goal y position [m] 54 | output: 55 | rx: x position list of the final path 56 | ry: y position list of the final path 57 | """ 58 | 59 | start_node = self.Node(self.calc_xy_index(sx, self.min_x), 60 | self.calc_xy_index(sy, self.min_y), 0.0, -1) 61 | goal_node = self.Node(self.calc_xy_index(gx, self.min_x), 62 | self.calc_xy_index(gy, self.min_y), 0.0, -1) 63 | 64 | open_set, closed_set = dict(), dict() 65 | open_set[self.calc_grid_index(start_node)] = start_node 66 | 67 | while 1: 68 | if len(open_set) == 0: 69 | print("Open set is empty..") 70 | break 71 | 72 | c_id = min( 73 | open_set, 74 | key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, 75 | open_set[ 76 | o])) 77 | current = open_set[c_id] 78 | 79 | # show graph 80 | if show_animation: # pragma: no cover 81 | plt.plot(self.calc_grid_position(current.x, self.min_x), 82 | self.calc_grid_position(current.y, self.min_y), "xc") 83 | # for stopping simulation with the esc key. 84 | plt.gcf().canvas.mpl_connect('key_release_event', 85 | lambda event: [exit( 86 | 0) if event.key == 'escape' else None]) 87 | if len(closed_set.keys()) % 10 == 0: 88 | plt.pause(0.001) 89 | 90 | if current.x == goal_node.x and current.y == goal_node.y: 91 | print("Find goal") 92 | goal_node.parent_index = current.parent_index 93 | goal_node.cost = current.cost 94 | break 95 | 96 | # Remove the item from the open set 97 | del open_set[c_id] 98 | 99 | # Add it to the closed set 100 | closed_set[c_id] = current 101 | 102 | # expand_grid search grid based on motion model 103 | for i, _ in enumerate(self.motion): 104 | node = self.Node(current.x + self.motion[i][0], 105 | current.y + self.motion[i][1], 106 | current.cost + self.motion[i][2], c_id) 107 | n_id = self.calc_grid_index(node) 108 | 109 | # If the node is not safe, do nothing 110 | if not self.verify_node(node): 111 | continue 112 | 113 | if n_id in closed_set: 114 | continue 115 | 116 | if n_id not in open_set: 117 | open_set[n_id] = node # discovered a new node 118 | else: 119 | if open_set[n_id].cost > node.cost: 120 | # This path is the best until now. record it 121 | open_set[n_id] = node 122 | 123 | rx, ry = self.calc_final_path(goal_node, closed_set) 124 | 125 | return rx, ry 126 | 127 | def calc_final_path(self, goal_node, closed_set): 128 | # generate final course 129 | rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ 130 | self.calc_grid_position(goal_node.y, self.min_y)] 131 | parent_index = goal_node.parent_index 132 | while parent_index != -1: 133 | n = closed_set[parent_index] 134 | rx.append(self.calc_grid_position(n.x, self.min_x)) 135 | ry.append(self.calc_grid_position(n.y, self.min_y)) 136 | parent_index = n.parent_index 137 | 138 | return rx, ry 139 | 140 | @staticmethod 141 | def calc_heuristic(n1, n2): 142 | w = 1.0 # weight of heuristic 143 | d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) 144 | return d 145 | 146 | def calc_grid_position(self, index, min_position): 147 | """ 148 | calc grid position 149 | :param index: 150 | :param min_position: 151 | :return: 152 | """ 153 | pos = index * self.resolution + min_position 154 | return pos 155 | 156 | def calc_xy_index(self, position, min_pos): 157 | return round((position - min_pos) / self.resolution) 158 | 159 | def calc_grid_index(self, node): 160 | return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) 161 | 162 | def verify_node(self, node): 163 | px = self.calc_grid_position(node.x, self.min_x) 164 | py = self.calc_grid_position(node.y, self.min_y) 165 | 166 | if px < self.min_x: 167 | return False 168 | elif py < self.min_y: 169 | return False 170 | elif px >= self.max_x: 171 | return False 172 | elif py >= self.max_y: 173 | return False 174 | 175 | # collision check 176 | if self.obstacle_map[node.x][node.y]: 177 | return False 178 | 179 | return True 180 | 181 | def calc_obstacle_map(self, ox, oy): 182 | 183 | self.min_x = round(min(ox)) 184 | self.min_y = round(min(oy)) 185 | self.max_x = round(max(ox)) 186 | self.max_y = round(max(oy)) 187 | print("min_x:", self.min_x) 188 | print("min_y:", self.min_y) 189 | print("max_x:", self.max_x) 190 | print("max_y:", self.max_y) 191 | 192 | self.x_width = round((self.max_x - self.min_x) / self.resolution) 193 | self.y_width = round((self.max_y - self.min_y) / self.resolution) 194 | print("x_width:", self.x_width) 195 | print("y_width:", self.y_width) 196 | 197 | # obstacle map generation 198 | self.obstacle_map = [[False for _ in range(self.y_width)] 199 | for _ in range(self.x_width)] 200 | for ix in range(self.x_width): 201 | x = self.calc_grid_position(ix, self.min_x) 202 | for iy in range(self.y_width): 203 | y = self.calc_grid_position(iy, self.min_y) 204 | for iox, ioy in zip(ox, oy): 205 | d = math.hypot(iox - x, ioy - y) 206 | if d <= self.rr: 207 | self.obstacle_map[ix][iy] = True 208 | break 209 | 210 | @staticmethod 211 | def get_motion_model(): 212 | # dx, dy, cost 213 | motion = [[1, 0, 1], 214 | [0, 1, 1], 215 | [-1, 0, 1], 216 | [0, -1, 1], 217 | [-1, -1, math.sqrt(2)], 218 | [-1, 1, math.sqrt(2)], 219 | [1, -1, math.sqrt(2)], 220 | [1, 1, math.sqrt(2)]] 221 | 222 | return motion 223 | 224 | 225 | def main(): 226 | print(__file__ + " start!!") 227 | 228 | # start and goal position 229 | sx = 10.0 # [m] 230 | sy = 10.0 # [m] 231 | gx = 50.0 # [m] 232 | gy = 50.0 # [m] 233 | grid_size = 2.0 # [m] 234 | robot_radius = 1.0 # [m] 235 | 236 | # set obstacle positions 237 | ox, oy = [], [] 238 | for i in range(-10, 60): 239 | ox.append(i) 240 | oy.append(-10.0) 241 | for i in range(-10, 60): 242 | ox.append(60.0) 243 | oy.append(i) 244 | for i in range(-10, 61): 245 | ox.append(i) 246 | oy.append(60.0) 247 | for i in range(-10, 61): 248 | ox.append(-10.0) 249 | oy.append(i) 250 | for i in range(-10, 40): 251 | ox.append(20.0) 252 | oy.append(i) 253 | for i in range(0, 40): 254 | ox.append(40.0) 255 | oy.append(60.0 - i) 256 | 257 | if show_animation: # pragma: no cover 258 | plt.plot(ox, oy, ".k") 259 | plt.plot(sx, sy, "og") 260 | plt.plot(gx, gy, "xb") 261 | plt.grid(True) 262 | plt.axis("equal") 263 | 264 | a_star = AStarPlanner(ox, oy, grid_size, robot_radius) 265 | rx, ry = a_star.planning(sx, sy, gx, gy) 266 | 267 | if show_animation: # pragma: no cover 268 | plt.plot(rx, ry, "-r") 269 | plt.pause(0.001) 270 | plt.show() 271 | 272 | 273 | if __name__ == '__main__': 274 | main() 275 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/AStar.py: -------------------------------------------------------------------------------- 1 | """ 2 | A* grid planning 3 | author: Atsushi Sakai(@Atsushi_twi) 4 | Nikos Kanargias (nkana@tee.gr) 5 | See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) 6 | """ 7 | 8 | import math 9 | 10 | import matplotlib.pyplot as plt 11 | 12 | show_animation = True 13 | 14 | 15 | class AStarPlanner: 16 | 17 | def __init__(self, ox, oy, resolution, rr): 18 | """ 19 | Initialize grid map for a star planning 20 | ox: x position list of Obstacles [m] 21 | oy: y position list of Obstacles [m] 22 | resolution: grid resolution [m] 23 | rr: robot radius[m] 24 | """ 25 | 26 | self.resolution = resolution 27 | self.rr = rr 28 | self.min_x, self.min_y = 0, 0 29 | self.max_x, self.max_y = 0, 0 30 | self.obstacle_map = None 31 | self.x_width, self.y_width = 0, 0 32 | self.motion = self.get_motion_model() 33 | self.calc_obstacle_map(ox, oy) 34 | 35 | class Node: 36 | def __init__(self, x, y, cost, parent_index): 37 | self.x = x # index of grid 38 | self.y = y # index of grid 39 | self.cost = cost 40 | self.parent_index = parent_index 41 | 42 | def __str__(self): 43 | return str(self.x) + "," + str(self.y) + "," + str( 44 | self.cost) + "," + str(self.parent_index) 45 | 46 | def planning(self, sx, sy, gx, gy): 47 | """ 48 | A star path search 49 | input: 50 | s_x: start x position [m] 51 | s_y: start y position [m] 52 | gx: goal x position [m] 53 | gy: goal y position [m] 54 | output: 55 | rx: x position list of the final path 56 | ry: y position list of the final path 57 | """ 58 | 59 | start_node = self.Node(self.calc_xy_index(sx, self.min_x), 60 | self.calc_xy_index(sy, self.min_y), 0.0, -1) 61 | goal_node = self.Node(self.calc_xy_index(gx, self.min_x), 62 | self.calc_xy_index(gy, self.min_y), 0.0, -1) 63 | 64 | open_set, closed_set = dict(), dict() 65 | open_set[self.calc_grid_index(start_node)] = start_node 66 | 67 | while 1: 68 | if len(open_set) == 0: 69 | print("Open set is empty..") 70 | break 71 | 72 | c_id = min( 73 | open_set, 74 | key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, 75 | open_set[ 76 | o])) 77 | current = open_set[c_id] 78 | 79 | # show graph 80 | if show_animation: # pragma: no cover 81 | plt.plot(self.calc_grid_position(current.x, self.min_x), 82 | self.calc_grid_position(current.y, self.min_y), "xc") 83 | # for stopping simulation with the esc key. 84 | plt.gcf().canvas.mpl_connect('key_release_event', 85 | lambda event: [exit( 86 | 0) if event.key == 'escape' else None]) 87 | if len(closed_set.keys()) % 10 == 0: 88 | plt.pause(0.001) 89 | 90 | if current.x == goal_node.x and current.y == goal_node.y: 91 | print("Find goal") 92 | goal_node.parent_index = current.parent_index 93 | goal_node.cost = current.cost 94 | break 95 | 96 | # Remove the item from the open set 97 | del open_set[c_id] 98 | 99 | # Add it to the closed set 100 | closed_set[c_id] = current 101 | 102 | # expand_grid search grid based on motion model 103 | for i, _ in enumerate(self.motion): 104 | node = self.Node(current.x + self.motion[i][0], 105 | current.y + self.motion[i][1], 106 | current.cost + self.motion[i][2], c_id) 107 | n_id = self.calc_grid_index(node) 108 | 109 | # If the node is not safe, do nothing 110 | if not self.verify_node(node): 111 | continue 112 | 113 | if n_id in closed_set: 114 | continue 115 | 116 | if n_id not in open_set: 117 | open_set[n_id] = node # discovered a new node 118 | else: 119 | if open_set[n_id].cost > node.cost: 120 | # This path is the best until now. record it 121 | open_set[n_id] = node 122 | 123 | rx, ry = self.calc_final_path(goal_node, closed_set) 124 | 125 | return rx, ry 126 | 127 | def calc_final_path(self, goal_node, closed_set): 128 | # generate final course 129 | rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ 130 | self.calc_grid_position(goal_node.y, self.min_y)] 131 | parent_index = goal_node.parent_index 132 | while parent_index != -1: 133 | n = closed_set[parent_index] 134 | rx.append(self.calc_grid_position(n.x, self.min_x)) 135 | ry.append(self.calc_grid_position(n.y, self.min_y)) 136 | parent_index = n.parent_index 137 | 138 | return rx, ry 139 | 140 | @staticmethod 141 | def calc_heuristic(n1, n2): 142 | w = 1.0 # weight of heuristic 143 | d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) 144 | return d 145 | 146 | def calc_grid_position(self, index, min_position): 147 | """ 148 | calc grid position 149 | :param index: 150 | :param min_position: 151 | :return: 152 | """ 153 | pos = index * self.resolution + min_position 154 | return pos 155 | 156 | def calc_xy_index(self, position, min_pos): 157 | return round((position - min_pos) / self.resolution) 158 | 159 | def calc_grid_index(self, node): 160 | return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) 161 | 162 | def verify_node(self, node): 163 | px = self.calc_grid_position(node.x, self.min_x) 164 | py = self.calc_grid_position(node.y, self.min_y) 165 | 166 | if px < self.min_x: 167 | return False 168 | elif py < self.min_y: 169 | return False 170 | elif px >= self.max_x: 171 | return False 172 | elif py >= self.max_y: 173 | return False 174 | 175 | # collision check 176 | if self.obstacle_map[node.x][node.y]: 177 | return False 178 | 179 | return True 180 | 181 | def calc_obstacle_map(self, ox, oy): 182 | 183 | self.min_x = round(min(ox)) 184 | self.min_y = round(min(oy)) 185 | self.max_x = round(max(ox)) 186 | self.max_y = round(max(oy)) 187 | print("min_x:", self.min_x) 188 | print("min_y:", self.min_y) 189 | print("max_x:", self.max_x) 190 | print("max_y:", self.max_y) 191 | 192 | self.x_width = round((self.max_x - self.min_x) / self.resolution) 193 | self.y_width = round((self.max_y - self.min_y) / self.resolution) 194 | print("x_width:", self.x_width) 195 | print("y_width:", self.y_width) 196 | 197 | # obstacle map generation 198 | self.obstacle_map = [[False for _ in range(self.y_width)] 199 | for _ in range(self.x_width)] 200 | for ix in range(self.x_width): 201 | x = self.calc_grid_position(ix, self.min_x) 202 | for iy in range(self.y_width): 203 | y = self.calc_grid_position(iy, self.min_y) 204 | for iox, ioy in zip(ox, oy): 205 | d = math.hypot(iox - x, ioy - y) 206 | if d <= self.rr: 207 | self.obstacle_map[ix][iy] = True 208 | break 209 | 210 | @staticmethod 211 | def get_motion_model(): 212 | # dx, dy, cost 213 | motion = [[1, 0, 1], 214 | [0, 1, 1], 215 | [-1, 0, 1], 216 | [0, -1, 1], 217 | [-1, -1, math.sqrt(2)], 218 | [-1, 1, math.sqrt(2)], 219 | [1, -1, math.sqrt(2)], 220 | [1, 1, math.sqrt(2)]] 221 | 222 | return motion 223 | 224 | 225 | def main(): 226 | print(__file__ + " start!!") 227 | 228 | # start and goal position 229 | sx = 10.0 # [m] 230 | sy = 10.0 # [m] 231 | gx = 50.0 # [m] 232 | gy = 5.0 # [m] 233 | grid_size = 2.0 # [m] 234 | robot_radius = 1.0 # [m] 235 | 236 | # set obstacle positions 237 | ox, oy = [], [] 238 | for i in range(-10, 60): 239 | ox.append(i) 240 | oy.append(-10.0) 241 | for i in range(-10, 60): 242 | ox.append(60.0) 243 | oy.append(i) 244 | for i in range(-10, 61): 245 | ox.append(i) 246 | oy.append(60.0) 247 | for i in range(-10, 61): 248 | ox.append(-10.0) 249 | oy.append(i) 250 | for i in range(-10, 40): 251 | ox.append(20.0) 252 | oy.append(i) 253 | for i in range(0, 40): 254 | ox.append(40.0) 255 | oy.append(60.0 - i) 256 | 257 | if show_animation: # pragma: no cover 258 | plt.plot(ox, oy, ".k") 259 | plt.plot(sx, sy, "og") 260 | plt.plot(gx, gy, "xb") 261 | plt.grid(True) 262 | plt.axis("equal") 263 | 264 | a_star = AStarPlanner(ox, oy, grid_size, robot_radius) 265 | rx, ry = a_star.planning(sx, sy, gx, gy) 266 | print(rx) 267 | if show_animation: # pragma: no cover 268 | plt.plot(rx, ry, "-r") 269 | plt.pause(0.001) 270 | plt.show() 271 | 272 | 273 | if __name__ == '__main__': 274 | main() 275 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/ControlDesignLQG16States.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Octave Packeges 7 | pkg load control; 8 | 9 | %% Mass of the Multirotor in Kilograms as taken from the CAD 10 | 11 | M = 0.857945; 12 | g = 9.81; 13 | 14 | %% Dimensions of Multirotor 15 | 16 | L = 0.16319; % along X-axis and Y-axis Distance from left and right motor pair to center of mass 17 | 18 | %% Mass Moment of Inertia as Taken from the CAD 19 | 20 | Ixx = 1.061E+05/(1000*10000); 21 | Iyy = 1.061E+05/(1000*10000); 22 | Izz = 2.011E+05/(1000*10000); 23 | 24 | %% Motor Thrust and Torque Constants (To be determined experimentally) 25 | 26 | Ktau = 7.708e-10 * 2; 27 | Kthrust = 1.812e-07; 28 | Kthrust2 = 0.0007326; 29 | Mtau = 1/44.22; 30 | Ku = 515.5; 31 | 32 | %% Equilibrium Input 33 | 34 | W_e = ((-4*Kthrust2) + sqrt((4*Kthrust2)^2 - (4*(-M*g)*(4*Kthrust))))/(2*(4*Kthrust))*ones(4,1); 35 | U_e = (W_e/(Ku*Mtau)); 36 | 37 | 38 | %% Define Discrete-Time BeagleBone Dynamics 39 | 40 | T = 0.01; % Sample period (s)- 100Hz 41 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 42 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 43 | 44 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 45 | 46 | % A = 16x16 matrix 47 | A = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 48 | 0, 0, 0, 0, 0, 0, 0, 0, -g, 0, 0, 0, 0, 0, 0, 0; 49 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 50 | 0, 0, 0, 0, 0, 0, g, 0, 0, 0, 0, 0, 0, 0, 0, 0; 51 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 52 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2*Kthrust*W_e(1)/M, -2*Kthrust*W_e(2)/M, -2*Kthrust*W_e(3)/M, -2*Kthrust*W_e(4)/M; 53 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0; 54 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, L*2*Kthrust*W_e(1)/Ixx, L*2*Kthrust*W_e(2)/Ixx, -L*2*Kthrust*W_e(3)/Ixx, -L*2*Kthrust*W_e(4)/Ixx; 55 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0; 56 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, L*2*Kthrust*W_e(1)/Iyy, -L*2*Kthrust*W_e(2)/Iyy, L*2*Kthrust*W_e(3)/Iyy, -L*2*Kthrust*W_e(4)/Iyy; 57 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; 58 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2*Ktau*W_e(1)/Izz, -2*Ktau*W_e(2)/Izz, -2*Ktau*W_e(3)/Izz, 2*Ktau*W_e(4)/Izz; 59 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0; 60 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0; 61 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0; 62 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau]; 63 | 64 | % B = 16x4 matrix 65 | B = [0, 0, 0, 0; 66 | 0, 0, 0, 0; 67 | 0, 0, 0, 0; 68 | 0, 0, 0, 0; 69 | 0, 0, 0, 0; 70 | 0, 0, 0, 0; 71 | 0, 0, 0, 0; 72 | 0, 0, 0, 0; 73 | 0, 0, 0, 0; 74 | 0, 0, 0, 0; 75 | 0, 0, 0, 0; 76 | 0, 0, 0, 0; 77 | Ku, 0, 0, 0; 78 | 0, Ku, 0, 0; 79 | 0, 0, Ku, 0; 80 | 0, 0, 0, Ku]; 81 | 82 | % C = 4x16 matrix 83 | C = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 84 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 85 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 86 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 87 | 88 | % D = 4x4 matrix 89 | D = [0, 0, 0, 0; 90 | 0, 0, 0, 0; 91 | 0, 0, 0, 0; 92 | 0, 0, 0, 0]; 93 | 94 | %% Discrete-Time System 95 | 96 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 97 | 98 | Adt = sysdt.a; 99 | Bdt = sysdt.b; 100 | Cdt = sysdt.c; 101 | Ddt = sysdt.d; 102 | 103 | %% System Characteristics 104 | 105 | poles = eig(Adt); 106 | % System Unstable 107 | 108 | figure(1); 109 | plot(poles,'*'); 110 | grid on; 111 | title('Discrete System Eigenvalues'); 112 | 113 | cntr = rank(ctrb(Adt,Bdt)); 114 | % Fully Reachable 115 | 116 | obs = rank(obsv(Adt,Cdt)); 117 | % Fully Observable 118 | 119 | %% Discrete-Time Full Integral Augmaneted System 120 | 121 | Cr = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 122 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 123 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 124 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 125 | 126 | u = size(B,2); % number of controlled inputs 127 | r = size(Cr,1); % number of reference inputs 128 | n = size(A,2); % number of states 129 | q = size(Cr,1); % number of controlled outputs 130 | 131 | Dr = zeros(q,u); 132 | 133 | Adtaug = [Adt zeros(n,r); 134 | -Cr*Adt eye(q,r)]; 135 | 136 | Bdtaug = [Bdt; 137 | -Cr*Bdt]; 138 | 139 | Cdtaug = [C zeros(r,r)]; 140 | 141 | %% Discrete-Time Full State-Feedback Control 142 | % State feedback control design with integral control via state augmentation 143 | % X Y Z Psi are controlled outputs 144 | 145 | Q = diag([5000,0,5000,0,5000,0,1000,0,1000,0,5000,0,0,0,0,0,5,5,5,0.4]); % State penalty 146 | %Q = diag([5,0,5,0,5,0,10,0,10,0,5,0,0,0,0,0,1,1,1,0.1]); % State penalty 147 | R = eye(4,4)*(10^-4); % Control penalty 148 | 149 | Kdtaug = dlqr(Adtaug,Bdtaug,Q,R); % DT State-Feedback Controller Gains 150 | Kdt = Kdtaug(:,1:n); % LQR Gains 151 | Kidt = Kdtaug(:,n+1:end); % Integral Gains 152 | 153 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 154 | 155 | Gdt = 1e-1*eye(n); 156 | 157 | Rw = diag([0.1,1,0.1,1,0.1,1,1,0.1,1,0.1,1,0.1,1000,1000,1000,1000]); % Process noise covariance matrix 158 | Rv = diag([10^-5,10^-5,10^-5,10^-5]); % Measurement noise covariance matrix Note: use low gausian noice for Rv 159 | 160 | Ldt = dlqe(Adt,Gdt,Cdt,Rw,Rv); 161 | 162 | %Hdt = zeros(size(Cy,1),size(Gdt,2)); % No process noise on measurements 163 | %sys4kf = ss(Adt,[Bdt Gdt],Cy,[Dy Hdt],T); 164 | %sys4kf = ss(Adt,Bdt,Cy,Dy,T); 165 | %[kalm,Ldt] = kalman(sys4kf,Rw,Rv); 166 | 167 | %% Dynamic Simulation 168 | 169 | Time = 1; 170 | kT = round(Time/T); 171 | 172 | X = zeros(16,kT); 173 | Xreal = zeros(16,kT); 174 | Xest = zeros(16,kT); 175 | Xe = zeros(4,kT); 176 | Y = zeros(4,kT); 177 | e = zeros(4,kT); 178 | 179 | U = ones(4,kT); 180 | U(:,1) = U_e; 181 | Ref = [0;0;0;0]; 182 | 183 | t_span = [0,T]; 184 | 185 | profile on; 186 | PROT = profile("info"); 187 | 188 | for k = 2:kT-1 189 | 190 | %Reference Setting 191 | if k == 10/T 192 | Ref(1) = 0.2; 193 | end 194 | if k == 15/T 195 | Ref(1) = 0; 196 | end 197 | if k == 20/T 198 | Ref(2) = 0.2; 199 | end 200 | if k == 25/T 201 | Ref(2) = 0; 202 | end 203 | if k == 30/T 204 | Ref(3) = -0.2; 205 | end 206 | if k == 35/T 207 | Ref(3) = -0.2; 208 | end 209 | if k == 40/T 210 | Ref(4) = 90*pi/180; 211 | end 212 | if k == 50/T 213 | Ref(4) = 0; 214 | end 215 | 216 | %Estimation 217 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % No KF Linear Prediction 218 | 219 | % Xest(:,k) = Xreal(:,k); % No KF Non Linear Prediction 220 | 221 | % Y(:,k) = Xreal([1,3,5,11],k); 222 | % xkf = [Xest(:,k-1)]; 223 | % xode = ode45(@(t,X) Quad_Dynamics(t,X,U(:,k-1)),t_span,xkf); % Nonlinear Prediction 224 | % Xest(:,k) = xode.y(:,end); 225 | % e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 226 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 227 | 228 | Y(:,k) = Xreal([1,3,5,11],k); 229 | Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Linear Prediction 230 | e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 231 | Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 232 | 233 | %Control 234 | Xe(:,k) = Xe(:,k-1) + (Ref - Xest([1,3,5,11],k)); % Integrator 235 | U(:,k) = min(800, max(0, U_e - [Kdt,Kidt]*[Xest(:,k) ;Xe(:,k)])); % Constraint Saturation 236 | 237 | %Simulation 238 | t_span = [0,T]; 239 | xode = ode45(@(t,X) Quad_Dynamics(t,X,U(:,k)),t_span,Xreal(:,k)); % Runge-Kutta Integration Nonlinear Dynamics 240 | Xreal(:,k+1) = xode.y(:,end); 241 | 242 | % t=t_span; 243 | % [dX] = Quad_Dynamics(t,Xreal(:,k),U(:,k)); % Forward Euler Integration Nonlinear Dynamics 244 | % Xreal(:,k+1) = Xreal(:,k) + T*dX; 245 | 246 | % Xreal(:,k+1) = Adt*Xreal(:,k) + Bdt*(U(:,k)-U_e); % Fully Linear Dynamics 247 | end 248 | 249 | PROT = profile("info"); 250 | profile off; 251 | 252 | Rad2Deg = [180/pi,180/pi,180/pi]'; 253 | 254 | %Plots 255 | t = (0:kT-1)*T; 256 | figure(2); 257 | subplot(2,1,1); 258 | plot(t,Xreal([1,3,5],:).*[1,1,-1]'); 259 | legend('X','Y','Z'); 260 | title('Real Position'); 261 | xlabel('Time(s)'); 262 | ylabel('Meters(m)'); 263 | 264 | subplot(2,1,2); 265 | plot(t,Xreal([7,9,11],:).*Rad2Deg); 266 | legend('\phi','\theta','\psi'); 267 | title('Real Attitude'); 268 | xlabel('Time(s)'); 269 | ylabel('Degrees(d)'); 270 | 271 | figure(3); 272 | subplot(2,1,1); 273 | plot(t,Xest([1,3,5],:).*[1,1,-1]'); 274 | legend('X_e','Y_e','Z_e'); 275 | title('Estimated Position'); 276 | xlabel('Time(s)'); 277 | ylabel('Meters(m)'); 278 | 279 | subplot(2,1,2); 280 | plot(t,Xest([7,9,11],:).*Rad2Deg); 281 | legend('\phi_e','\theta_e','\psi_e'); 282 | title('Estimated Attitude'); 283 | xlabel('Time(s)'); 284 | ylabel('Degrees(d)'); 285 | 286 | figure(4); 287 | subplot(2,1,1); 288 | plot(t,e([1,2,3],:)); 289 | legend('e_x','e_y','e_z'); 290 | title('Position prediction error'); 291 | xlabel('Time(s)'); 292 | ylabel('Error meters(m)'); 293 | 294 | subplot(2,1,2); 295 | plot(t,e(4,:)*Rad2Deg(1)); 296 | legend('e_\psi'); 297 | title('Hedding prediction error'); 298 | xlabel('Time(s)'); 299 | ylabel('Error degrees(d)'); 300 | 301 | figure(5); 302 | plot(t,U); 303 | legend('U1','U2','U3','U4'); 304 | title('Inputs PWM Signal'); 305 | xlabel('Time(s)'); 306 | ylabel('Micro Seconds(ms)'); 307 | 308 | Cpoles = eig(Adtaug - (Bdtaug*[Kdt,Kidt])); 309 | figure(6); 310 | plot(Cpoles,'*'); 311 | grid on; 312 | title('Closed-Loop Eigenvalues'); 313 | 314 | Obspoles = eig(Adt - (Ldt*Cdt)); 315 | figure(7); 316 | plot(Obspoles,'*'); 317 | grid on; 318 | title('Closed-Loop Estimator Eigenvalues'); 319 | 320 | %% PRINT TO CONFIGURATION FILES 321 | 322 | dlmwrite("Adt.txt", Adt,',', 0, 0); 323 | dlmwrite("Bdt.txt", Bdt,',', 0, 0); 324 | dlmwrite("Cdt.txt", Cdt,',', 0, 0); 325 | dlmwrite("Ddt.txt", Ddt,',', 0, 0); 326 | dlmwrite("Kdt.txt", Kdt,',', 0, 0); 327 | dlmwrite("Kidt.txt", Kidt,',', 0, 0); 328 | dlmwrite("Ldt.txt", Ldt,',', 0, 0); 329 | dlmwrite("U_e.txt", U_e,',', 0, 0); -------------------------------------------------------------------------------- /Planning/RRTStar.py: -------------------------------------------------------------------------------- 1 | """ 2 | Path planning Sample Code with RRT* 3 | author: Atsushi Sakai(@Atsushi_twi) 4 | """ 5 | 6 | import math 7 | import os 8 | import sys 9 | 10 | import matplotlib.pyplot as plt 11 | 12 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRT/") 13 | 14 | try: 15 | from rrt import RRT 16 | except ImportError: 17 | raise 18 | 19 | show_animation = True 20 | 21 | 22 | class RRTStar(RRT): 23 | """ 24 | Class for RRT Star planning 25 | """ 26 | 27 | class Node(RRT.Node): 28 | def __init__(self, x, y): 29 | super().__init__(x, y) 30 | self.cost = 0.0 31 | 32 | def __init__(self, 33 | start, 34 | goal, 35 | obstacle_list, 36 | rand_area, 37 | expand_dis=30.0, 38 | path_resolution=1.0, 39 | goal_sample_rate=20, 40 | max_iter=300, 41 | connect_circle_dist=50.0, 42 | search_until_max_iter=False): 43 | """ 44 | Setting Parameter 45 | start:Start Position [x,y] 46 | goal:Goal Position [x,y] 47 | obstacleList:obstacle Positions [[x,y,size],...] 48 | randArea:Random Sampling Area [min,max] 49 | """ 50 | super().__init__(start, goal, obstacle_list, rand_area, expand_dis, 51 | path_resolution, goal_sample_rate, max_iter) 52 | self.connect_circle_dist = connect_circle_dist 53 | self.goal_node = self.Node(goal[0], goal[1]) 54 | self.search_until_max_iter = search_until_max_iter 55 | 56 | def planning(self, animation=True): 57 | """ 58 | rrt star path planning 59 | animation: flag for animation on or off . 60 | """ 61 | 62 | self.node_list = [self.start] 63 | for i in range(self.max_iter): 64 | print("Iter:", i, ", number of nodes:", len(self.node_list)) 65 | rnd = self.get_random_node() 66 | nearest_ind = self.get_nearest_node_index(self.node_list, rnd) 67 | new_node = self.steer(self.node_list[nearest_ind], rnd, 68 | self.expand_dis) 69 | near_node = self.node_list[nearest_ind] 70 | new_node.cost = near_node.cost + \ 71 | math.hypot(new_node.x-near_node.x, 72 | new_node.y-near_node.y) 73 | 74 | if self.check_collision(new_node, self.obstacle_list): 75 | near_inds = self.find_near_nodes(new_node) 76 | node_with_updated_parent = self.choose_parent( 77 | new_node, near_inds) 78 | if node_with_updated_parent: 79 | self.rewire(node_with_updated_parent, near_inds) 80 | self.node_list.append(node_with_updated_parent) 81 | else: 82 | self.node_list.append(new_node) 83 | 84 | if animation: 85 | self.draw_graph(rnd) 86 | 87 | if ((not self.search_until_max_iter) 88 | and new_node): # if reaches goal 89 | last_index = self.search_best_goal_node() 90 | if last_index is not None: 91 | return self.generate_final_course(last_index) 92 | 93 | print("reached max iteration") 94 | 95 | last_index = self.search_best_goal_node() 96 | if last_index is not None: 97 | return self.generate_final_course(last_index) 98 | 99 | return None 100 | 101 | def choose_parent(self, new_node, near_inds): 102 | """ 103 | Computes the cheapest point to new_node contained in the list 104 | near_inds and set such a node as the parent of new_node. 105 | Arguments: 106 | -------- 107 | new_node, Node 108 | randomly generated node with a path from its neared point 109 | There are not coalitions between this node and th tree. 110 | near_inds: list 111 | Indices of indices of the nodes what are near to new_node 112 | Returns. 113 | ------ 114 | Node, a copy of new_node 115 | """ 116 | if not near_inds: 117 | return None 118 | 119 | # search nearest cost in near_inds 120 | costs = [] 121 | for i in near_inds: 122 | near_node = self.node_list[i] 123 | t_node = self.steer(near_node, new_node) 124 | if t_node and self.check_collision(t_node, self.obstacle_list): 125 | costs.append(self.calc_new_cost(near_node, new_node)) 126 | else: 127 | costs.append(float("inf")) # the cost of collision node 128 | min_cost = min(costs) 129 | 130 | if min_cost == float("inf"): 131 | print("There is no good path.(min_cost is inf)") 132 | return None 133 | 134 | min_ind = near_inds[costs.index(min_cost)] 135 | new_node = self.steer(self.node_list[min_ind], new_node) 136 | new_node.cost = min_cost 137 | 138 | return new_node 139 | 140 | def search_best_goal_node(self): 141 | dist_to_goal_list = [ 142 | self.calc_dist_to_goal(n.x, n.y) for n in self.node_list 143 | ] 144 | goal_inds = [ 145 | dist_to_goal_list.index(i) for i in dist_to_goal_list 146 | if i <= self.expand_dis 147 | ] 148 | 149 | safe_goal_inds = [] 150 | for goal_ind in goal_inds: 151 | t_node = self.steer(self.node_list[goal_ind], self.goal_node) 152 | if self.check_collision(t_node, self.obstacle_list): 153 | safe_goal_inds.append(goal_ind) 154 | 155 | if not safe_goal_inds: 156 | return None 157 | 158 | min_cost = min([self.node_list[i].cost for i in safe_goal_inds]) 159 | for i in safe_goal_inds: 160 | if self.node_list[i].cost == min_cost: 161 | return i 162 | 163 | return None 164 | 165 | def find_near_nodes(self, new_node): 166 | """ 167 | 1) defines a ball centered on new_node 168 | 2) Returns all nodes of the three that are inside this ball 169 | Arguments: 170 | --------- 171 | new_node: Node 172 | new randomly generated node, without collisions between 173 | its nearest node 174 | Returns: 175 | ------- 176 | list 177 | List with the indices of the nodes inside the ball of 178 | radius r 179 | """ 180 | nnode = len(self.node_list) + 1 181 | r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode)) 182 | # if expand_dist exists, search vertices in a range no more than 183 | # expand_dist 184 | if hasattr(self, 'expand_dis'): 185 | r = min(r, self.expand_dis) 186 | dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2 187 | for node in self.node_list] 188 | near_inds = [dist_list.index(i) for i in dist_list if i <= r**2] 189 | return near_inds 190 | 191 | def rewire(self, new_node, near_inds): 192 | """ 193 | For each node in near_inds, this will check if it is cheaper to 194 | arrive to them from new_node. 195 | In such a case, this will re-assign the parent of the nodes in 196 | near_inds to new_node. 197 | Parameters: 198 | ---------- 199 | new_node, Node 200 | Node randomly added which can be joined to the tree 201 | near_inds, list of uints 202 | A list of indices of the self.new_node which contains 203 | nodes within a circle of a given radius. 204 | Remark: parent is designated in choose_parent. 205 | """ 206 | for i in near_inds: 207 | near_node = self.node_list[i] 208 | edge_node = self.steer(new_node, near_node) 209 | if not edge_node: 210 | continue 211 | edge_node.cost = self.calc_new_cost(new_node, near_node) 212 | 213 | no_collision = self.check_collision(edge_node, self.obstacle_list) 214 | improved_cost = near_node.cost > edge_node.cost 215 | 216 | if no_collision and improved_cost: 217 | near_node.x = edge_node.x 218 | near_node.y = edge_node.y 219 | near_node.cost = edge_node.cost 220 | near_node.path_x = edge_node.path_x 221 | near_node.path_y = edge_node.path_y 222 | near_node.parent = edge_node.parent 223 | self.propagate_cost_to_leaves(new_node) 224 | 225 | def calc_new_cost(self, from_node, to_node): 226 | d, _ = self.calc_distance_and_angle(from_node, to_node) 227 | return from_node.cost + d 228 | 229 | def propagate_cost_to_leaves(self, parent_node): 230 | 231 | for node in self.node_list: 232 | if node.parent == parent_node: 233 | node.cost = self.calc_new_cost(parent_node, node) 234 | self.propagate_cost_to_leaves(node) 235 | 236 | 237 | def main(): 238 | print("Start " + __file__) 239 | 240 | # ====Search Path with RRT==== 241 | obstacle_list = [ 242 | (5, 5, 1), 243 | (3, 6, 2), 244 | (3, 8, 2), 245 | (3, 10, 2), 246 | (7, 5, 2), 247 | (9, 5, 2), 248 | (8, 10, 1), 249 | (6, 12, 1), 250 | ] # [x,y,size(radius)] 251 | 252 | # Set Initial parameters 253 | rrt_star = RRTStar( 254 | start=[0, 0], 255 | goal=[6, 10], 256 | rand_area=[-2, 15], 257 | obstacle_list=obstacle_list, 258 | expand_dis=1) 259 | path = rrt_star.planning(animation=show_animation) 260 | 261 | if path is None: 262 | print("Cannot find path") 263 | else: 264 | print("found path!!") 265 | 266 | # Draw final path 267 | if show_animation: 268 | rrt_star.draw_graph() 269 | plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--') 270 | plt.grid(True) 271 | plt.show() 272 | 273 | 274 | if __name__ == '__main__': 275 | main() 276 | -------------------------------------------------------------------------------- /X4 LQG Controller/16States/EKF_Test.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Octave Packeges 7 | pkg load control; 8 | pkg load optim; 9 | %% Mass of the Multirotor in Kilograms as taken from the CAD 10 | 11 | M = 0.857945; 12 | g = 9.81; 13 | 14 | %% Dimensions of Multirotor 15 | 16 | L = 0.16319; % along X-axis and Y-axis Distance from left and right motor pair to center of mass 17 | 18 | %% Mass Moment of Inertia as Taken from the CAD 19 | 20 | Ixx = 1.061E+05/(1000*10000); 21 | Iyy = 1.061E+05/(1000*10000); 22 | Izz = 2.011E+05/(1000*10000); 23 | 24 | %% Motor Thrust and Torque Constants (To be determined experimentally) 25 | 26 | Ktau = 7.708e-10 * 2; 27 | Kthrust = 1.812e-07; 28 | Kthrust2 = 0.0007326; 29 | Mtau = (1/44.22); 30 | Ku = 515.5; 31 | 32 | %% Equilibrium Input 33 | 34 | W_e = ((-4*Kthrust2) + sqrt((4*Kthrust2)^2 - (4*(-M*g)*(4*Kthrust))))/(2*(4*Kthrust))*ones(4,1); 35 | U_e = (W_e/(Ku*Mtau)); 36 | 37 | 38 | %% Define Discrete-Time BeagleBone Dynamics 39 | 40 | T = 0.01; % Sample period (s)- 100Hz 41 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 42 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 43 | 44 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 45 | 46 | % A = 16x16 matrix 47 | A = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 48 | 0, 0, 0, 0, 0, 0, 0, 0, -g, 0, 0, 0, 0, 0, 0, 0; 49 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 50 | 0, 0, 0, 0, 0, 0, g, 0, 0, 0, 0, 0, 0, 0, 0, 0; 51 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 52 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -2*Kthrust*W_e(1)/M, -2*Kthrust*W_e(2)/M, -2*Kthrust*W_e(3)/M, -2*Kthrust*W_e(4)/M; 53 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0; 54 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, L*2*Kthrust*W_e(1)/Ixx, L*2*Kthrust*W_e(2)/Ixx, -L*2*Kthrust*W_e(3)/Ixx, -L*2*Kthrust*W_e(4)/Ixx; 55 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0; 56 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, L*2*Kthrust*W_e(1)/Iyy, -L*2*Kthrust*W_e(2)/Iyy, L*2*Kthrust*W_e(3)/Iyy, -L*2*Kthrust*W_e(4)/Iyy; 57 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0; 58 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2*Ktau*W_e(1)/Izz, -2*Ktau*W_e(2)/Izz, -2*Ktau*W_e(3)/Izz, 2*Ktau*W_e(4)/Izz; 59 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0; 60 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0; 61 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0; 62 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau]; 63 | 64 | % B = 16x4 matrix 65 | B = [0, 0, 0, 0; 66 | 0, 0, 0, 0; 67 | 0, 0, 0, 0; 68 | 0, 0, 0, 0; 69 | 0, 0, 0, 0; 70 | 0, 0, 0, 0; 71 | 0, 0, 0, 0; 72 | 0, 0, 0, 0; 73 | 0, 0, 0, 0; 74 | 0, 0, 0, 0; 75 | 0, 0, 0, 0; 76 | 0, 0, 0, 0; 77 | Ku, 0, 0, 0; 78 | 0, Ku, 0, 0; 79 | 0, 0, Ku, 0; 80 | 0, 0, 0, Ku]; 81 | 82 | % C = 4x16 matrix 83 | C = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 84 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 85 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 86 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 87 | 88 | % D = 4x4 matrix 89 | D = [0, 0, 0, 0; 90 | 0, 0, 0, 0; 91 | 0, 0, 0, 0; 92 | 0, 0, 0, 0]; 93 | 94 | %% Discrete-Time System 95 | 96 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 97 | 98 | Adt = sysdt.a; 99 | Bdt = sysdt.b; 100 | Cdt = sysdt.c; 101 | Ddt = sysdt.d; 102 | 103 | %% System Characteristics 104 | 105 | poles = eig(Adt); 106 | % System Unstable 107 | 108 | figure(1) 109 | plot(poles,'*') 110 | grid on 111 | title('Discrete System Eigenvalues') 112 | 113 | cntr = rank(ctrb(Adt,Bdt)); 114 | % Fully Reachable 115 | 116 | obs = rank(obsv(Adt,Cdt)); 117 | % Fully Observable 118 | 119 | %% Discrete-Time Full Integral Augmaneted System 120 | 121 | Cr = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 122 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 123 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 124 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; 125 | 126 | u = size(B,2); % number of controlled inputs 127 | r = size(Cr,1); % number of reference inputs 128 | n = size(A,2); % number of states 129 | q = size(Cr,1); % number of controlled outputs 130 | 131 | Dr = zeros(q,u); 132 | 133 | Adtaug = [Adt zeros(n,r); 134 | -Cr*Adt eye(q,r)]; 135 | 136 | Bdtaug = [Bdt; 137 | -Cr*Bdt]; 138 | 139 | Cdtaug = [C zeros(r,r)]; 140 | 141 | %% Discrete-Time Full State-Feedback Control 142 | % State feedback control design with integral control via state augmentation 143 | % X Y Z Psi are controlled outputs 144 | 145 | Q = diag([5000,0,5000,0,5000,0,1000,0,1000,0,5000,0,0,0,0,0,5,5,5,0.4]); % State penalty 146 | %Q = diag([5,0,5,0,5,0,10,0,10,0,5,0,0,0,0,0,1,1,1,0.1]); % State penalty 147 | R = eye(4,4)*(10^-4); % Control penalty 148 | 149 | Kdtaug = dlqr(Adtaug,Bdtaug,Q,R); % DT State-Feedback Controller Gains 150 | Kdt = Kdtaug(:,1:n); % LQR Gains 151 | Kidt = Kdtaug(:,n+1:end); % Integral Gains 152 | 153 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 154 | 155 | Gdt = 1e-1*eye(n); 156 | 157 | Rw = diag([0.1,1,0.1,1,0.1,1,1,0.1,1,0.1,1,0.1,1000,1000,1000,1000]); % Process noise covariance matrix 158 | Rv = diag([10^-5,10^-5,10^-5,10^-5]); % Measurement noise covariance matrix Note: use low gausian noice for Rv 159 | 160 | Ldt = dlqe(Adt,Gdt,Cdt,Rw,Rv); 161 | 162 | %Hdt = zeros(size(Cy,1),size(Gdt,2)); % No process noise on measurements 163 | %sys4kf = ss(Adt,[Bdt Gdt],Cy,[Dy Hdt],T); 164 | %sys4kf = ss(Adt,Bdt,Cy,Dy,T); 165 | %[kalm,Ldt] = kalman(sys4kf,Rw,Rv); 166 | 167 | %% Dynamic Simulation 168 | 169 | Time = 60; 170 | kT = round(Time/T); 171 | 172 | X = zeros(16,kT); 173 | Xreal = zeros(16,kT); 174 | Xest = zeros(16,kT); 175 | Xe = zeros(4,kT); 176 | Y = zeros(4,kT); 177 | e = zeros(4,kT); 178 | 179 | U = ones(4,kT); 180 | U(:,1) = U_e; 181 | Ref = [0;0;0;0]; 182 | 183 | t_span = [0,T]; 184 | 185 | profile on; 186 | PROT = profile("info"); 187 | 188 | for k = 2:kT-1 189 | 190 | %Reference Setting 191 | if k == 10/T 192 | Ref(1) = 0.2; 193 | end 194 | if k == 15/T 195 | Ref(1) = 0; 196 | end 197 | if k == 20/T 198 | Ref(2) = 0.2; 199 | end 200 | if k == 25/T 201 | Ref(2) = 0; 202 | end 203 | if k == 30/T 204 | Ref(3) = -0.2; 205 | end 206 | if k == 35/T 207 | Ref(3) = -0.2; 208 | end 209 | if k == 40/T 210 | Ref(4) = 90*pi/180; 211 | end 212 | if k == 50/T 213 | Ref(4) = 0; 214 | end 215 | 216 | %Estimation 217 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % No KF Linear Prediction 218 | 219 | % Xest(:,k) = Xreal(:,k); % No KF Non Linear Prediction 220 | 221 | % Y(:,k) = Xreal([1,3,5,11],k); 222 | % xkf = [Xest(:,k-1)]; 223 | % xode = ode45(@(t,X) Quad_Dynamics(t,X,U(:,k-1)),t_span,xkf); % Nonlinear Prediction 224 | % Xest(:,k) = xode.y(:,end); 225 | % e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 226 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 227 | 228 | t = 0; 229 | Df = jacobs (Xest(:,k-1), @(t,X) Quad_Dynamics(t,Xest(:,k-1),U(:,k-1))); 230 | LdtEkf = dlqe(Df,Gdt,Cdt,Rw,Rv); 231 | Y(:,k) = Xreal([1,3,5,11],k); 232 | Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Extended Kalman Prediction 233 | e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 234 | Xest(:,k) = Xest(:,k) + LdtEkf*e(:,k); 235 | 236 | % Y(:,k) = Xreal([1,3,5,11],k); 237 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Linear Kalman Prediction 238 | % e(:,k) = [Y(:,k) - Xest([1,3,5,11],k)]; 239 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 240 | 241 | %Control 242 | Xe(:,k) = Xe(:,k-1) + (Ref - Xest([1,3,5,11],k)); % Integrator 243 | U(:,k) = min(800, max(0, U_e - [Kdt,Kidt]*[Xest(:,k) ;Xe(:,k)])); % Constraint Saturation 244 | 245 | %Simulation 246 | t_span = [0,T]; 247 | xode = ode45(@(t,X) Quad_Dynamics(t,X,U(:,k)),t_span,Xreal(:,k)); % Runge-Kutta Integration Nonlinear Dynamics 248 | Xreal(:,k+1) = xode.y(:,end); 249 | 250 | % t=t_span; 251 | % [dX] = Quad_Dynamics(t,Xreal(:,k),U(:,k)); % Forward Euler Integration Nonlinear Dynamics 252 | % Xreal(:,k+1) = Xreal(:,k) + T*dX; 253 | 254 | % Xreal(:,k+1) = Adt*Xreal(:,k) + Bdt*(U(:,k)-U_e); % Fully Linear Dynamics 255 | end 256 | 257 | PROT = profile("info"); 258 | profile off; 259 | 260 | Rad2Deg = [180/pi,180/pi,180/pi]'; 261 | 262 | %Plots 263 | t = (0:kT-1)*T; 264 | figure(2) 265 | subplot(2,1,1) 266 | plot(t,Xreal([1,3,5],:).*[1,1,-1]') 267 | legend('X','Y','Z') 268 | title('Real Position') 269 | xlabel('Time(s)') 270 | ylabel('Meters(m)') 271 | 272 | subplot(2,1,2) 273 | plot(t,Xreal([7,9,11],:).*Rad2Deg) 274 | legend('\phi','\theta','\psi') 275 | title('Real Attitude') 276 | xlabel('Time(s)') 277 | ylabel('Degrees(d)') 278 | 279 | figure(3) 280 | subplot(2,1,1) 281 | plot(t,Xest([1,3,5],:).*[1,1,-1]') 282 | legend('X_e','Y_e','Z_e') 283 | title('Estimated Position') 284 | xlabel('Time(s)') 285 | ylabel('Meters(m)') 286 | 287 | subplot(2,1,2) 288 | plot(t,Xest([7,9,11],:).*Rad2Deg) 289 | legend('\phi_e','\theta_e','\psi_e') 290 | title('Estimated Attitude') 291 | xlabel('Time(s)') 292 | ylabel('Degrees(d)') 293 | 294 | figure(4) 295 | subplot(2,1,1) 296 | plot(t,e([1,2,3],:)) 297 | legend('e_x','e_y','e_z') 298 | title('Position prediction error') 299 | xlabel('Time(s)') 300 | ylabel('Error meters(m)') 301 | 302 | subplot(2,1,2) 303 | plot(t,e(4,:)*Rad2Deg(1)) 304 | legend('e_\psi') 305 | title('Hedding prediction error') 306 | xlabel('Time(s)') 307 | ylabel('Error degrees(d)') 308 | 309 | figure(5) 310 | plot(t,U) 311 | legend('U1','U2','U3','U4') 312 | title('Inputs PWM Signal') 313 | xlabel('Time(s)') 314 | ylabel('Micro Seconds(ms)') 315 | 316 | Cpoles = eig(Adtaug - (Bdtaug*[Kdt,Kidt])); 317 | figure(6) 318 | plot(Cpoles,'*') 319 | grid on 320 | title('Closed-Loop Eigenvalues') 321 | 322 | Obspoles = eig(Adt - (Ldt*Cdt)); 323 | figure(7) 324 | plot(Obspoles,'*') 325 | grid on 326 | title('Closed-Loop Estimator Eigenvalues') 327 | 328 | %% PRINT TO CONFIGURATION FILES 329 | 330 | dlmwrite("Adt.txt", Adt,',', 0, 0) 331 | dlmwrite("Bdt.txt", Bdt,',', 0, 0) 332 | dlmwrite("Cdt.txt", Cdt,',', 0, 0) 333 | dlmwrite("Ddt.txt", Ddt,',', 0, 0) 334 | dlmwrite("Kdt.txt", Kdt,',', 0, 0) 335 | dlmwrite("Kidt.txt", Kidt,',', 0, 0) 336 | dlmwrite("Ldt.txt", Ldt,',', 0, 0) 337 | dlmwrite("U_e.txt", U_e,',', 0, 0) -------------------------------------------------------------------------------- /Y6 LQG Controller/ControlDesignLQG14States.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Octave Packeges 7 | pkg load control; 8 | 9 | %% Mass of the Multirotor in Kilograms as taken from the CAD 10 | 11 | M = 1.455882; 12 | g = 9.81; 13 | 14 | %% Dimensions of Multirotor 15 | 16 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 17 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 18 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 19 | 20 | %% Mass Moment of Inertia as Taken from the CAD 21 | 22 | Ixx = 0.014; 23 | Iyy = 0.028; 24 | Izz = 0.038; 25 | 26 | %% Motor Thrust and Torque Constants (To be determined experimentally) 27 | 28 | Kw = 0.85; 29 | Ktau = 7.708e-10; 30 | Kthrust = 1.812e-07; 31 | Kthrust2 = 0.0007326; 32 | Mtau = (1/44.22); 33 | Ku = 515.5*Mtau; 34 | 35 | %% Air resistance damping coeeficients 36 | 37 | Dxx = 0.01212; 38 | Dyy = 0.01212; 39 | Dzz = 0.0648; 40 | 41 | %% Equilibrium Input 42 | 43 | %W_e = sqrt(((M*g)/(3*(Kthrust+(Kw*Kthrust))))); 44 | %W_e = ((-6*Kthrust2) + sqrt((6*Kthrust2)^2 - (4*(-M*g)*(3*Kw*Kthrust + 3*Kthrust))))/(2*(3*Kw*Kthrust + 3*Kthrust)); 45 | %U_e = (W_e/Ku); 46 | U_e = [176.1,178.5,177.2,177.6,202.2,204.5]'; 47 | W_e = U_e*Ku; 48 | W_eV = [0;0;0;0;0;0;0;0;W_e]; 49 | 50 | %% Define Discrete-Time BeagleBone Dynamics 51 | 52 | T = 0.01; % Sample period (s)- 100Hz 53 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 54 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 55 | 56 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 57 | 58 | % A = 14x14 matrix 59 | A = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 60 | 0, 0, 0, 0, 0, 0, 0, 0, 2*Kthrust*W_e(1)/M, 2*Kw*Kthrust*W_e(2)/M, 2*Kthrust*W_e(3)/M, 2*Kw*Kthrust*W_e(4)/M, 2*Kthrust*W_e(5)/M, 2*Kw*Kthrust*W_e(6)/M; 61 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 62 | 0, 0, 0, 0, 0, 0, 0, 0, L1*2*Kthrust*W_e(1)/Ixx, L1*2*Kw*Kthrust*W_e(2)/Ixx, -L1*2*Kthrust*W_e(3)/Ixx, -L1*2*Kw*Kthrust*W_e(4)/Ixx, 0, 0; 63 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0; 64 | 0, 0, 0, 0, 0, 0, 0, 0, -L2*2*Kthrust*W_e(1)/Iyy, -L2*2*Kw*Kthrust*W_e(2)/Iyy, -L2*2*Kthrust*W_e(3)/Iyy, -L2*2*Kw*Kthrust*W_e(4)/Iyy, L3*2*Kthrust*W_e(5)/Iyy,L3*2*Kw*Kthrust*W_e(6)/Iyy; 65 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0; 66 | 0, 0, 0, 0, 0, 0, 0, 0, -2*Ktau*W_e(1)/Izz, 2*Ktau*W_e(2)/Izz, 2*Ktau*W_e(3)/Izz, -2*Ktau*W_e(4)/Izz, -2*Ktau*W_e(5)/Izz, 2*Ktau*W_e(6)/Izz; 67 | 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0, 0, 0; 68 | 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0, 0; 69 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0; 70 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0; 71 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0; 72 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau]; 73 | 74 | % B = 14x6 matrix 75 | B = [0, 0, 0, 0, 0, 0; 76 | 0, 0, 0, 0, 0, 0; 77 | 0, 0, 0, 0, 0, 0; 78 | 0, 0, 0, 0, 0, 0; 79 | 0, 0, 0, 0, 0, 0; 80 | 0, 0, 0, 0, 0, 0; 81 | 0, 0, 0, 0, 0, 0; 82 | 0, 0, 0, 0, 0, 0; 83 | Ku/Mtau, 0, 0, 0, 0, 0; 84 | 0, Ku/Mtau, 0, 0, 0, 0; 85 | 0, 0, Ku/Mtau, 0, 0, 0; 86 | 0, 0, 0, Ku/Mtau, 0, 0; 87 | 0, 0, 0, 0, Ku/Mtau, 0; 88 | 0, 0, 0, 0, 0, Ku/Mtau]; 89 | 90 | % C = 4x14 matrix 91 | C = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 92 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 93 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 94 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]; 95 | 96 | % D = 4x6 matrix 97 | D = 0; 98 | 99 | %% Discrete-Time System 100 | 101 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 102 | 103 | Adt = sysdt.a; 104 | Bdt = sysdt.b; 105 | Cdt = sysdt.c; 106 | Ddt = sysdt.d; 107 | 108 | %% System Characteristics 109 | 110 | poles = eig(Adt); 111 | % System Unstable 112 | 113 | figure(1) 114 | plot(poles,'*') 115 | grid on 116 | title('Discrete System Eigenvalues') 117 | 118 | cntr = rank(ctrb(Adt,Bdt)); 119 | % Fully Reachable 120 | 121 | obs = rank(obsv(Adt,Cdt)); 122 | % Partially Observable but Detectable 123 | 124 | %% Discrete-Time Full Integral Augmaneted System 125 | 126 | Cr = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 127 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 128 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 129 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]; 130 | 131 | r = 4; % number of reference inputs 132 | n = size(A,2); % number of states 133 | q = size(Cr,1); % number of controlled outputs 134 | 135 | Dr = zeros(q,6); 136 | 137 | Adtaug = [Adt zeros(n,r); 138 | -Cr*Adt eye(q,r)]; 139 | 140 | Bdtaug = [Bdt; -Cr*Bdt]; 141 | 142 | Cdtaug = [C zeros(r,r)]; 143 | 144 | %% Discrete-Time Full State-Feedback Control 145 | % State feedback control design with integral control via state augmentation 146 | % Z Phi Theta Psi are controlled outputs 147 | 148 | Q = diag([5000,1000,10000,0,10000,0,1000,200,0,0,0,0,0,0,5,1000,1000,1]); % State penalty 149 | R = (1*10^-3)*eye(6,6); % Control penalty 150 | 151 | Kdtaug = dlqr(Adtaug,Bdtaug,Q,R); % DT State-Feedback Controller Gains 152 | Kdt = Kdtaug(:,1:n); % LQR Gains 153 | Kidt = Kdtaug(:,n+1:end); % Integral Gains 154 | 155 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 156 | % Utilises a 6 output model with motor 1 and 4 velocities being used as 157 | % virtual aditiional outputs. This is due to layout coupling leading to many different 158 | % combintaions of motor velocity producing the same outcome, therefore 159 | % making the system unobservale otherwise, therefore to combat this, 160 | % the model is assumed to be perfect with reguargdes to the 2 motors and 161 | % 0 error is fedback to the KF gains for the motors 162 | 163 | Cy = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 164 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 165 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 166 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0; 167 | 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0; 168 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0]; 169 | 170 | Dy = zeros(6,6); 171 | 172 | Gdt = 1e-1*eye(n); 173 | Hdt = zeros(size(Cy,1),size(Gdt,2)); % No process noise on measurements 174 | 175 | Rw = diag([0.5,0.5,0.01,0.1,0.01,0.01,0.01,0.01,10^-10,10^-10,10^-10,10^-10,10^-10,10^-10]); % Process noise covariance matrix 176 | Rv = diag([500,10^-5,10^-5,10^-5,10^-5,10^-5]); % Measurement noise covariance matrix Note: use low gausian noice for Rv 177 | 178 | sys4kf = ss(Adt,[Bdt Gdt],Cy,[Dy Hdt],T); 179 | Ldt = dlqe(Adt,Gdt,Cy,Rw,Rv); 180 | %sys4kf = ss(Adt,Bdt,Cy,Dy,T); 181 | %[kalm,Ldt] = kalman(sys4kf,Rw,Rv); 182 | 183 | %% Dynamic Simulation 184 | 185 | Time = 50; 186 | kT = round(Time/T); 187 | 188 | X = zeros(14,kT); 189 | Xreal = zeros(18,kT); 190 | Xe = zeros(4,kT); 191 | Y = zeros(4,kT); 192 | e = zeros(6,kT); 193 | 194 | U = ones(6,kT); 195 | U(:,1) = U_e; 196 | 197 | Ref = [0;0;0;0]; 198 | x_ini = [0;0;0;0;0;0;0;0;0;0;0;0;0;0]; 199 | 200 | X(:,2) = x_ini; 201 | Xest = X; 202 | Xest(:,1) = x_ini+0.001*randn(14,1); 203 | Xreal(5:end,2) = x_ini; 204 | 205 | t_span = [0,T]; 206 | 207 | for k = 2:kT-1 208 | 209 | %%Reference Setting 210 | if k == 10/T 211 | Ref(1) = 1; 212 | end 213 | if k == 15/T 214 | Ref(2) = 30*pi/180; 215 | end 216 | if k == 20/T 217 | Ref(2) = 0; 218 | end 219 | if k == 25/T 220 | Ref(3) = 30*pi/180; 221 | end 222 | if k == 30/T 223 | Ref(3) = 0; 224 | end 225 | if k == 40/T 226 | Ref(4) = 90*pi/180; 227 | end 228 | 229 | %%Estimation 230 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % No KF Linear Prediction 231 | 232 | % Xest(:,k) = Xreal([5,6,7,8,9,10,11,12,13:18],k); % No KF Non Linear Prediction 233 | 234 | Y(:,k) = Xreal([5,7,9,11],k); 235 | xkf = [0;0;0;0;Xest(:,k-1)]; 236 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k-1)),t_span,xkf); % Nonlinear Prediction 237 | Xest(:,k) = xode.y(5:18,end); 238 | e(:,k) = [Y(:,k) - Xest([1,3,5,7],k); 0; 0]; 239 | Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 240 | 241 | % Y(:,k) = Xreal([5,7,9,11],k); 242 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Linear Prediction 243 | % e(:,k) = [Y(:,k) - Xest([1,3,5,7],k); 0; 0]; 244 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 245 | 246 | %%Control 247 | Xe(:,k) = Xe(:,k-1) + (Ref - Xest([1,3,5,7],k)); % Integrator 248 | %U(:,k) = U_e - [Kdt,Kidt]*[Xest(:,k); Xe(:,k)]; 249 | U(:,k) = min(800, max(0, U_e - [Kdt,Kidt]*[Xest(:,k); Xe(:,k)])); % Constraint Saturation 250 | 251 | %Simulation 252 | t_span = [0,T]; 253 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k)),t_span,Xreal(:,k)); % Runge-Kutta Integration Nonlinear Dynamics 254 | Xreal(:,k+1) = xode.y(:,end); 255 | 256 | % [dX] = Hex_Dynamics(t,Xreal(:,k),U(:,k)); % Forward Euler Integration Nonlinear Dynamics 257 | % Xreal(:,k+1) = Xreal(:,k)+T*dX; 258 | 259 | % X(:,k+1) = Adt*X(:,k) + Bdt*U(:,k); % Fully Linear Dynamics 260 | end 261 | 262 | Rad2Deg = [180/pi,180/pi,180/pi]'; 263 | 264 | %Plots 265 | t = (0:kT-1)*T; 266 | figure(2) 267 | subplot(2,1,1) 268 | plot(t,Xreal(5,:)) 269 | legend('Alt') 270 | title('Real Altitude') 271 | xlabel('Time(s)') 272 | ylabel('Meters(m)') 273 | 274 | subplot(2,1,2) 275 | plot(t,Xreal([7,9,11],:).*Rad2Deg) 276 | legend('\phi','\theta','\psi') 277 | title('Real Attitude') 278 | xlabel('Time(s)') 279 | ylabel('Degrees(d)') 280 | 281 | figure(3) 282 | subplot(2,1,1) 283 | plot(t,Xest(1,:)) 284 | legend('Alt_e') 285 | title('Estimated Altitude') 286 | xlabel('Time(s)') 287 | ylabel('Meters(m)') 288 | 289 | subplot(2,1,2) 290 | plot(t,Xest([3,5,7],:).*Rad2Deg) 291 | legend('\phi_e','\theta_e','\psi_e') 292 | title('Estimated Attitude') 293 | xlabel('Time(s)') 294 | ylabel('Degrees(d)') 295 | 296 | figure(4) 297 | subplot(2,1,1) 298 | plot(t,e(1,:)) 299 | legend('e_z') 300 | title('Altitude prediction error') 301 | xlabel('Time(s)') 302 | ylabel('Error meters(m)') 303 | 304 | subplot(2,1,2) 305 | plot(t,e([2,3,4],:).*Rad2Deg) 306 | legend('e_\phi','e_\theta','e_\psi') 307 | title('Attitude prediction error') 308 | xlabel('Time(s)') 309 | ylabel('Error degrees(d)') 310 | 311 | figure(5) 312 | plot(t,U) 313 | legend('U1','U2','U3','U4','U5','U6') 314 | title('Inputs PWM Signal') 315 | xlabel('Time(s)') 316 | ylabel('Micro Seconds(ms)') 317 | 318 | % Cpoles = eig(Adtaug - (Bdtaug*[Kdt,Kidt])); 319 | % % System Unstable 320 | % 321 | % figure(6) 322 | % plot(Cpoles(1:14),'*') 323 | % grid on 324 | % title('Closed-Loop Eigenvalues') 325 | 326 | %% PRINT TO CONFIGURATION FILES 327 | 328 | %K = round([Kdt,Kidt],2); 329 | 330 | %writematrix(round(Kdt,2),'config.txt','Delimiter','comma') 331 | 332 | %writematrix(round(Kidt,2),'config2.txt','Delimiter','comma') 333 | 334 | %writematrix(round(Ldt,2),'config3.txt','Delimiter','comma') -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MHE_code/MHE_Robot_PS_mul_shooting_v1.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.1.1 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 10; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include at the initial state of the robot and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % make the decision variable one column vector 49 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 50 | 51 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 52 | 53 | opts = struct; 54 | opts.ipopt.max_iter = 2000; 55 | opts.ipopt.print_level =0;%0,3 56 | opts.print_time = 0; 57 | opts.ipopt.acceptable_tol =1e-8; 58 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 59 | 60 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 61 | 62 | 63 | args = struct; 64 | 65 | args.lbg(1:3*(N+1)) = 0; 66 | args.ubg(1:3*(N+1)) = 0; 67 | 68 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 69 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 70 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 71 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 72 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 73 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 74 | 75 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 76 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 77 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 78 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 79 | %---------------------------------------------- 80 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 81 | 82 | 83 | % THE SIMULATION LOOP SHOULD START FROM HERE 84 | %------------------------------------------- 85 | t0 = 0; 86 | x0 = [0.1 ; 0.1 ; 0.0]; % initial condition. 87 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 88 | 89 | xx(:,1) = x0; % xx contains the history of states 90 | t(1) = t0; 91 | 92 | u0 = zeros(N,2); % two control inputs for each robot 93 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 94 | 95 | sim_tim = 20; % total sampling times 96 | 97 | % Start MPC 98 | mpciter = 0; 99 | xx1 = []; 100 | u_cl=[]; 101 | 102 | % the main simulaton loop... it works as long as the error is greater 103 | % than 10^-6 and the number of mpc steps is less than its maximum 104 | % value. 105 | tic 106 | while(norm((x0-xs),2) > 0.05 && mpciter < sim_tim / T) 107 | args.p = [x0;xs]; % set the values of the parameters vector 108 | % initial value of the optimization variables 109 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 110 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 111 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 112 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 113 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 114 | u_cl= [u_cl ; u(1,:)]; 115 | t(mpciter+1) = t0; 116 | % Apply the control and shift the solution 117 | [t0, x0, u0] = shift(T, t0, x0, u,f); 118 | xx(:,mpciter+2) = x0; 119 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 120 | % Shift trajectory to initialize the next step 121 | X0 = [X0(2:end,:);X0(end,:)]; 122 | mpciter 123 | mpciter = mpciter + 1; 124 | end; 125 | toc 126 | 127 | ss_error = norm((x0-xs),2) 128 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 129 | %----------------------------------------- 130 | %----------------------------------------- 131 | %----------------------------------------- 132 | % Start MHE implementation from here 133 | %----------------------------------------- 134 | %----------------------------------------- 135 | %----------------------------------------- 136 | % plot the ground truth 137 | figure(1) 138 | subplot(311) 139 | plot(t,xx(1,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 140 | ylabel('x (m)') 141 | grid on 142 | subplot(312) 143 | plot(t,xx(2,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 144 | ylabel('y (m)') 145 | grid on 146 | subplot(313) 147 | plot(t,xx(3,1:end-1),'b','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 148 | xlabel('time (seconds)') 149 | ylabel('\theta (rad)') 150 | grid on 151 | 152 | % Synthesize the measurments 153 | con_cov = diag([0.005 deg2rad(2)]).^2; 154 | meas_cov = diag([0.1 deg2rad(2)]).^2; 155 | 156 | r = []; 157 | alpha = []; 158 | for k = 1: length(xx(1,:))-1 159 | r = [r; sqrt(xx(1,k)^2+xx(2,k)^2) + sqrt(meas_cov(1,1))*randn(1)]; 160 | alpha = [alpha; atan(xx(2,k)/xx(1,k)) + sqrt(meas_cov(2,2))*randn(1)]; 161 | end 162 | y_measurements = [ r , alpha ]; 163 | 164 | % Plot the cartesian coordinates from the measurements used 165 | figure(1) 166 | subplot(311) 167 | plot(t,r.*cos(alpha),'r','linewidth',1.5); hold on 168 | grid on 169 | legend('Ground Truth','Measurement') 170 | subplot(312) 171 | plot(t,r.*sin(alpha),'r','linewidth',1.5); hold on 172 | grid on 173 | 174 | % plot the ground truth mesurements VS the noisy measurements 175 | figure(2) 176 | subplot(211) 177 | plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on 178 | plot(t,r,'r','linewidth',1.5); axis([0 t(end) -0.2 3]); hold on 179 | ylabel('Range: [ r (m) ]') 180 | grid on 181 | legend('Ground Truth','Measurement') 182 | subplot(212) 183 | plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on 184 | plot(t,alpha,'r','linewidth',1.5); axis([0 t(end) 0.2 1]); hold on 185 | ylabel('Bearing: [ \alpha (rad) ]') 186 | grid on 187 | 188 | % The following two matrices contain what we know about the system, i.e. 189 | % the nominal control actions applied (measured) and the range and bearing 190 | % measurements. 191 | %------------------------------------------------------------------------- 192 | u_cl; 193 | y_measurements; 194 | 195 | % Let's now formulate our MHE problem 196 | %------------------------------------ 197 | T = 0.2; %[s] 198 | N_MHE = size(y_measurements,1)-1; % Estimation horizon 199 | 200 | v_max = 0.6; v_min = -v_max; 201 | omega_max = pi/4; omega_min = -omega_max; 202 | 203 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 204 | states = [x;y;theta]; n_states = length(states); 205 | v = SX.sym('v'); omega = SX.sym('omega'); 206 | controls = [v;omega]; n_controls = length(controls); 207 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 208 | f = Function('f',{states,controls},{rhs}); % MOTION MODEL 209 | 210 | r = SX.sym('r'); alpha = SX.sym('alpha'); % range and bearing 211 | measurement_rhs = [sqrt(x^2+y^2); atan(y/x)]; 212 | h = Function('h',{states},{measurement_rhs}); % MEASUREMENT MODEL 213 | %y_tilde = [r;alpha]; 214 | 215 | % Decision variables 216 | U = SX.sym('U',n_controls,N_MHE); %(controls) 217 | X = SX.sym('X',n_states,(N_MHE+1)); %(states) [remember multiple shooting] 218 | 219 | P = SX.sym('P', 2 , (N_MHE+1) + N_MHE); 220 | % parameters (include r and alpha measurements as well as controls measurements) 221 | 222 | V = inv(sqrt(meas_cov)); % weighing matrices (output) y_tilde - y 223 | W = inv(sqrt(con_cov)); % weighing matrices (input) u_tilde - u 224 | 225 | obj = 0; % Objective function 226 | g = []; % constraints vector 227 | for k = 1:N_MHE+1 228 | st = X(:,k); 229 | h_x = h(st); 230 | y_tilde = P(:,k); 231 | obj = obj+ (y_tilde-h_x)' * V * (y_tilde-h_x); % calculate obj 232 | end 233 | 234 | for k = 1:N_MHE 235 | con = U(:,k); 236 | u_tilde = P(:,N_MHE+ k); 237 | obj = obj+ (u_tilde-con)' * W * (u_tilde-con); % calculate obj 238 | end 239 | 240 | % multiple shooting constraints 241 | for k = 1:N_MHE 242 | st = X(:,k); con = U(:,k); 243 | st_next = X(:,k+1); 244 | f_value = f(st,con); 245 | st_next_euler = st+ (T*f_value); 246 | g = [g;st_next-st_next_euler]; % compute constraints 247 | end 248 | 249 | 250 | % make the decision variable one column vector 251 | OPT_variables = [reshape(X,3*(N_MHE+1),1);reshape(U,2*N_MHE,1)]; 252 | 253 | nlp_mhe = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 254 | 255 | opts = struct; 256 | opts.ipopt.max_iter = 2000; 257 | opts.ipopt.print_level =0;%0,3 258 | opts.print_time = 0; 259 | opts.ipopt.acceptable_tol =1e-8; 260 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 261 | 262 | solver = nlpsol('solver', 'ipopt', nlp_mhe,opts); 263 | 264 | args = struct; 265 | 266 | args.lbg(1:3*(N_MHE)) = 0; % equality constraints 267 | args.ubg(1:3*(N_MHE)) = 0; % equality constraints 268 | 269 | args.lbx(1:3:3*(N_MHE+1),1) = -2; %state x lower bound 270 | args.ubx(1:3:3*(N_MHE+1),1) = 2; %state x upper bound 271 | args.lbx(2:3:3*(N_MHE+1),1) = -2; %state y lower bound 272 | args.ubx(2:3:3*(N_MHE+1),1) = 2; %state y upper bound 273 | args.lbx(3:3:3*(N_MHE+1),1) = -pi/2; %state theta lower bound 274 | args.ubx(3:3:3*(N_MHE+1),1) = pi/2; %state theta upper bound 275 | 276 | args.lbx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_min; %v lower bound 277 | args.ubx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_max; %v upper bound 278 | args.lbx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_min; %omega lower bound 279 | args.ubx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_max; %omega upper bound 280 | %---------------------------------------------- 281 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 282 | 283 | % MHE Simulation 284 | %------------------------------------------ 285 | U0 = zeros(N_MHE,2); % two control inputs for each robot 286 | X0 = zeros(N_MHE+1,3); % initialization of the states decision variables 287 | 288 | U0 = u_cl(1:N_MHE,:); % initialize the control actions by the measured 289 | % initialize the states from the measured range and bearing 290 | X0(:,1:2) = [y_measurements(1:N_MHE+1,1).*cos(y_measurements(1:N_MHE+1,2)),... 291 | y_measurements(1:N_MHE+1,1).*sin(y_measurements(1:N_MHE+1,2))]; 292 | 293 | k=1; 294 | % Get the measurements window and put it as parameters in p 295 | args.p = [y_measurements(k:k+N_MHE,:)',u_cl(k:k+N_MHE-1,:)']; 296 | % initial value of the optimization variables 297 | args.x0 = [reshape(X0',3*(N_MHE+1),1);reshape(U0',2*N_MHE,1)]; 298 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 299 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 300 | U_sol = reshape(full(sol.x(3*(N_MHE+1)+1:end))',2,N_MHE)'; % get controls only from the solution 301 | X_sol = reshape(full(sol.x(1:3*(N_MHE+1)))',3,N_MHE+1)'; % get solution TRAJECTORY 302 | 303 | figure(1) 304 | subplot(311) 305 | plot(t,X_sol(:,1),'g','linewidth',1.5); hold on 306 | legend('Ground Truth','Measurement','MHE') 307 | subplot(312) 308 | plot(t,X_sol(:,2),'g','linewidth',1.5); hold on 309 | subplot(313) 310 | plot(t,X_sol(:,3),'g','linewidth',1.5); hold on 311 | 312 | 313 | -------------------------------------------------------------------------------- /MPC-and-MHE-implementation-in-MATLAB-using-Casadi-master/Codes_casadi_v3_4_5/MHE_code/MHE_Robot_PS_mul_shooting_v2.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.1.1 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 10; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include at the initial state of the robot and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % make the decision variable one column vector 49 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 50 | 51 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 52 | 53 | opts = struct; 54 | opts.ipopt.max_iter = 2000; 55 | opts.ipopt.print_level =0;%0,3 56 | opts.print_time = 0; 57 | opts.ipopt.acceptable_tol =1e-8; 58 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 59 | 60 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 61 | 62 | 63 | args = struct; 64 | 65 | args.lbg(1:3*(N+1)) = 0; 66 | args.ubg(1:3*(N+1)) = 0; 67 | 68 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 69 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 70 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 71 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 72 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 73 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 74 | 75 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 76 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 77 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 78 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 79 | %---------------------------------------------- 80 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 81 | 82 | 83 | % THE SIMULATION LOOP SHOULD START FROM HERE 84 | %------------------------------------------- 85 | t0 = 0; 86 | x0 = [0.1 ; 0.1 ; 0.0]; % initial condition. 87 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 88 | 89 | xx(:,1) = x0; % xx contains the history of states 90 | t(1) = t0; 91 | 92 | u0 = zeros(N,2); % two control inputs for each robot 93 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 94 | 95 | sim_tim = 20; % total sampling times 96 | 97 | % Start MPC 98 | mpciter = 0; 99 | xx1 = []; 100 | u_cl=[]; 101 | 102 | % the main simulaton loop... it works as long as the error is greater 103 | % than 10^-6 and the number of mpc steps is less than its maximum 104 | % value. 105 | tic 106 | while(norm((x0-xs),2) > 0.05 && mpciter < sim_tim / T) 107 | args.p = [x0;xs]; % set the values of the parameters vector 108 | % initial value of the optimization variables 109 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 110 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 111 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 112 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 113 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 114 | u_cl= [u_cl ; u(1,:)]; 115 | t(mpciter+1) = t0; 116 | % Apply the control and shift the solution 117 | [t0, x0, u0] = shift(T, t0, x0, u,f); 118 | xx(:,mpciter+2) = x0; 119 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 120 | % Shift trajectory to initialize the next step 121 | X0 = [X0(2:end,:);X0(end,:)]; 122 | mpciter 123 | mpciter = mpciter + 1; 124 | end; 125 | toc 126 | 127 | ss_error = norm((x0-xs),2) 128 | %Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 129 | %----------------------------------------- 130 | %----------------------------------------- 131 | %----------------------------------------- 132 | % Start MHE implementation from here 133 | %----------------------------------------- 134 | %----------------------------------------- 135 | %----------------------------------------- 136 | % plot the ground truth 137 | figure(1) 138 | subplot(311) 139 | plot(t,xx(1,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 140 | ylabel('x (m)') 141 | grid on 142 | subplot(312) 143 | plot(t,xx(2,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 144 | ylabel('y (m)') 145 | grid on 146 | subplot(313) 147 | plot(t,xx(3,1:end-1),'b','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 148 | xlabel('time (seconds)') 149 | ylabel('\theta (rad)') 150 | grid on 151 | 152 | % Synthesize the measurments 153 | con_cov = diag([0.005 deg2rad(2)]).^2; 154 | meas_cov = diag([0.1 deg2rad(2)]).^2; 155 | 156 | r = []; 157 | alpha = []; 158 | for k = 1: length(xx(1,:))-1 159 | r = [r; sqrt(xx(1,k)^2+xx(2,k)^2) + sqrt(meas_cov(1,1))*randn(1)]; 160 | alpha = [alpha; atan(xx(2,k)/xx(1,k)) + sqrt(meas_cov(2,2))*randn(1)]; 161 | end 162 | y_measurements = [ r , alpha ]; 163 | 164 | % Plot the cartesian coordinates from the measurements used 165 | figure(1) 166 | subplot(311) 167 | plot(t,r.*cos(alpha),'r','linewidth',1.5); hold on 168 | grid on 169 | legend('Ground Truth','Measurement') 170 | subplot(312) 171 | plot(t,r.*sin(alpha),'r','linewidth',1.5); hold on 172 | grid on 173 | 174 | % plot the ground truth mesurements VS the noisy measurements 175 | figure(2) 176 | subplot(211) 177 | plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on 178 | plot(t,r,'r','linewidth',1.5); axis([0 t(end) -0.2 3]); hold on 179 | ylabel('Range: [ r (m) ]') 180 | grid on 181 | legend('Ground Truth','Measurement') 182 | subplot(212) 183 | plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on 184 | plot(t,alpha,'r','linewidth',1.5); axis([0 t(end) 0.2 1]); hold on 185 | ylabel('Bearing: [ \alpha (rad) ]') 186 | grid on 187 | 188 | % The following two matrices contain what we know about the system, i.e. 189 | % the nominal control actions applied (measured) and the range and bearing 190 | % measurements. 191 | %------------------------------------------------------------------------- 192 | u_cl; 193 | y_measurements; 194 | 195 | % Let's now formulate our MHE problem 196 | %------------------------------------ 197 | T = 0.2; %[s] 198 | N_MHE = 6; % prediction horizon 199 | 200 | v_max = 0.6; v_min = -v_max; 201 | omega_max = pi/4; omega_min = -omega_max; 202 | 203 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 204 | states = [x;y;theta]; n_states = length(states); 205 | v = SX.sym('v'); omega = SX.sym('omega'); 206 | controls = [v;omega]; n_controls = length(controls); 207 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 208 | f = Function('f',{states,controls},{rhs}); % MOTION MODEL 209 | 210 | r = SX.sym('r'); alpha = SX.sym('alpha'); % range and bearing 211 | measurement_rhs = [sqrt(x^2+y^2); atan(y/x)]; 212 | h = Function('h',{states},{measurement_rhs}); % MEASUREMENT MODEL 213 | %y_tilde = [r;alpha]; 214 | 215 | % Decision variables 216 | U = SX.sym('U',n_controls,N_MHE); %(controls) 217 | X = SX.sym('X',n_states,(N_MHE+1)); %(states) [remember multiple shooting] 218 | 219 | P = SX.sym('P', 2 , N_MHE + (N_MHE+1)); 220 | % parameters (include r and alpha measurements as well as controls measurements) 221 | 222 | V = inv(sqrt(meas_cov)); % weighing matrices (output) y_tilde - y 223 | W = inv(sqrt(con_cov)); % weighing matrices (input) u_tilde - u 224 | 225 | obj = 0; % Objective function 226 | g = []; % constraints vector 227 | for k = 1:N_MHE+1 228 | st = X(:,k); 229 | h_x = h(st); 230 | y_tilde = P(:,k); 231 | obj = obj+ (y_tilde-h_x)' * V * (y_tilde-h_x); % calculate obj 232 | end 233 | 234 | for k = 1:N_MHE 235 | con = U(:,k); 236 | u_tilde = P(:,N_MHE+ k); 237 | obj = obj+ (u_tilde-con)' * W * (u_tilde-con); % calculate obj 238 | end 239 | 240 | % multiple shooting constraints 241 | for k = 1:N_MHE 242 | st = X(:,k); con = U(:,k); 243 | st_next = X(:,k+1); 244 | f_value = f(st,con); 245 | st_next_euler = st+ (T*f_value); 246 | g = [g;st_next-st_next_euler]; % compute constraints 247 | end 248 | 249 | 250 | % make the decision variable one column vector 251 | OPT_variables = [reshape(X,3*(N_MHE+1),1);reshape(U,2*N_MHE,1)]; 252 | 253 | nlp_mhe = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 254 | 255 | opts = struct; 256 | opts.ipopt.max_iter = 2000; 257 | opts.ipopt.print_level =0;%0,3 258 | opts.print_time = 0; 259 | opts.ipopt.acceptable_tol =1e-8; 260 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 261 | 262 | solver = nlpsol('solver', 'ipopt', nlp_mhe,opts); 263 | 264 | 265 | args = struct; 266 | 267 | args.lbg(1:3*(N_MHE)) = 0; 268 | args.ubg(1:3*(N_MHE)) = 0; 269 | 270 | args.lbx(1:3:3*(N_MHE+1),1) = -2; %state x lower bound 271 | args.ubx(1:3:3*(N_MHE+1),1) = 2; %state x upper bound 272 | args.lbx(2:3:3*(N_MHE+1),1) = -2; %state y lower bound 273 | args.ubx(2:3:3*(N_MHE+1),1) = 2; %state y upper bound 274 | args.lbx(3:3:3*(N_MHE+1),1) = -pi/2; %state theta lower bound 275 | args.ubx(3:3:3*(N_MHE+1),1) = pi/2; %state theta upper bound 276 | 277 | args.lbx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_min; %v lower bound 278 | args.ubx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_max; %v upper bound 279 | args.lbx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_min; %omega lower bound 280 | args.ubx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_max; %omega upper bound 281 | %---------------------------------------------- 282 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 283 | 284 | % MHE Simulation loop starts here 285 | %------------------------------------------ 286 | X_estimate = []; % X_estimate contains the MHE estimate of the states 287 | U_estimate = []; % U_estimate contains the MHE estimate of the controls 288 | 289 | U0 = zeros(N_MHE,2); % two control inputs for each robot 290 | X0 = zeros(N_MHE+1,3); % initialization of the states decision variables 291 | 292 | % Start MHE 293 | mheiter = 0; 294 | 295 | U0 = u_cl(1:N_MHE,:); % initialize the control actions by the measured 296 | % initialize the states from the measured range and bearing 297 | X0(:,1:2) = [y_measurements(1:N_MHE+1,1).*cos(y_measurements(1:N_MHE+1,2)),... 298 | y_measurements(1:N_MHE+1,1).*sin(y_measurements(1:N_MHE+1,2))]; 299 | 300 | tic 301 | for k = 1: size(y_measurements,1) - (N_MHE) 302 | mheiter = k 303 | % Get the measurements window and put it as parameters in p 304 | args.p = [y_measurements(k:k+N_MHE,:)',u_cl(k:k+N_MHE-1,:)']; 305 | % initial value of the optimization variables 306 | args.x0 = [reshape(X0',3*(N_MHE+1),1);reshape(U0',2*N_MHE,1)]; 307 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 308 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 309 | U_sol = reshape(full(sol.x(3*(N_MHE+1)+1:end))',2,N_MHE)'; % get controls only from the solution 310 | X_sol = reshape(full(sol.x(1:3*(N_MHE+1)))',3,N_MHE+1)'; % get solution TRAJECTORY 311 | X_estimate = [X_estimate;X_sol(N_MHE+1,:)]; 312 | U_estimate = [U_estimate;U_sol(N_MHE,:)]; 313 | 314 | % Shift trajectory to initialize the next step 315 | X0 = [X_sol(2:end,:);X_sol(end,:)]; 316 | U0 = [U_sol(2:end,:);U_sol(end,:)]; 317 | end; 318 | toc 319 | 320 | figure(1) 321 | subplot(311) 322 | plot(t(N_MHE+1:end),X_estimate(:,1),'g','linewidth',1.5); hold on 323 | legend('Ground Truth','Measurement','MHE') 324 | 325 | subplot(312) 326 | plot(t(N_MHE+1:end),X_estimate(:,2),'g','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 327 | 328 | subplot(313) 329 | plot(t(N_MHE+1:end),X_estimate(:,3),'g','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 330 | 331 | 332 | --------------------------------------------------------------------------------