├── Documentation ├── Modelling, Simulation and Implementation of Linear Control for asymmetric Multirotor UAVs.pptx ├── Modelling, Simulation, and Implementation of Linear Control for Asymmetric Multirotor Unmanned Aerial Vehicles.pdf ├── Submitted Final Report Document.pdf ├── Y6UAV1.jpg ├── Y6UAV2.jpg └── Y6UAV3.jpg ├── Flight Control Software ├── LQG_Controller.c ├── Makefile └── Model_Parameters.h ├── LQG-14States ├── ControlDesignLQG14States.m ├── Hex_Dynamics.m └── Y6MultirotorLQRSimulation14State.slx ├── LQG-8States ├── ControlDesignLQG8States.m ├── Hex_Dynamics.m └── Y6MultirotorLQGSimulation8State.slx ├── MPC-14States ├── ControlDesignMPC14States.m ├── Hex_Dynamics.m ├── constraint_mats.m ├── constraint_mats_cl.m ├── cost_mats.m ├── cost_mats_cl.m ├── findmas.m ├── ompc_constraints.m ├── ompc_cost.m ├── ompc_simulate_constraints.m ├── ompc_suboptcost.m ├── predict_mats.m └── predict_mats_cl.m ├── Modelling ├── Dynamics.m ├── EquationsOfMotion.m ├── Hex_Dynamics.m ├── Motor Test Stand │ ├── DATA.TXT │ ├── ForceRPMdata.TXT │ ├── LoadData.TXT │ ├── MotorData.m │ ├── NoLoadDATA.TXT │ ├── TorqueData.TXT │ └── Y6ForceTorqueRPM │ │ └── Y6ForceTorqueRPM.ino ├── Simulation.m ├── Tracking_Example.m ├── Y6MultirotorOpenLoopModelSimulation.slx ├── Y6MultirotorOpenLoopModelSimulation2.slx └── plotUAV.m ├── PID-8States ├── ControlDesignPID.m ├── Hex_Dynamics.m └── Y6MultirotorPIDSimulation.slx └── README.md /Documentation/Modelling, Simulation and Implementation of Linear Control for asymmetric Multirotor UAVs.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Documentation/Modelling, Simulation and Implementation of Linear Control for asymmetric Multirotor UAVs.pptx -------------------------------------------------------------------------------- /Documentation/Modelling, Simulation, and Implementation of Linear Control for Asymmetric Multirotor Unmanned Aerial Vehicles.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Documentation/Modelling, Simulation, and Implementation of Linear Control for Asymmetric Multirotor Unmanned Aerial Vehicles.pdf -------------------------------------------------------------------------------- /Documentation/Submitted Final Report Document.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Documentation/Submitted Final Report Document.pdf -------------------------------------------------------------------------------- /Documentation/Y6UAV1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Documentation/Y6UAV1.jpg -------------------------------------------------------------------------------- /Documentation/Y6UAV2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Documentation/Y6UAV2.jpg -------------------------------------------------------------------------------- /Documentation/Y6UAV3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Documentation/Y6UAV3.jpg -------------------------------------------------------------------------------- /Flight Control Software/LQG_Controller.c: -------------------------------------------------------------------------------- 1 | /** 2 | * @file LQG_Controller.c 3 | * 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | #include // for atoi() and exit() 10 | #include // for M_PI 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "Model_Parameters.h" 21 | 22 | // I2C Pins 23 | #define I2C_BUS 2 24 | #define GPIO_INT_PIN_CHIP 3 25 | #define GPIO_INT_PIN_PIN 21 26 | 27 | // Filter Parameters 28 | #define SAMPLE_RATE 100 // hz 29 | #define DT (1.0/SAMPLE_RATE) 30 | #define PRINT_HZ 100 31 | #define BMP_RATE_DIV 100 // optionally sample bmp less frequently than mpu 32 | 33 | // Sensor Data Structures 34 | static rc_mpu_config_t conf; 35 | static rc_mpu_data_t mpu_data; 36 | static rc_bmp_data_t bmp_data; 37 | double innit_alt; 38 | 39 | // Kalman Data Structures 40 | static rc_kalman_t kf = RC_KALMAN_INITIALIZER; 41 | 42 | static rc_matrix_t F = RC_MATRIX_INITIALIZER; 43 | static rc_matrix_t G = RC_MATRIX_INITIALIZER; 44 | static rc_matrix_t H = RC_MATRIX_INITIALIZER; 45 | static rc_matrix_t Q = RC_MATRIX_INITIALIZER; 46 | static rc_matrix_t R = RC_MATRIX_INITIALIZER; 47 | static rc_matrix_t Pi = RC_MATRIX_INITIALIZER; 48 | 49 | // Reference-Error-Input-Output Data Structures 50 | static rc_vector_t r = RC_VECTOR_INITIALIZER; 51 | static rc_vector_t xe = RC_VECTOR_INITIALIZER; 52 | static rc_vector_t u = RC_VECTOR_INITIALIZER; 53 | static rc_vector_t u1 = RC_VECTOR_INITIALIZER; 54 | static rc_vector_t u2 = RC_VECTOR_INITIALIZER; 55 | static rc_vector_t y = RC_VECTOR_INITIALIZER; 56 | static rc_vector_t ye = RC_VECTOR_INITIALIZER; 57 | 58 | // System Gains Data Structures 59 | static rc_matrix_t K = RC_MATRIX_INITIALIZER; 60 | static rc_matrix_t Ki = RC_MATRIX_INITIALIZER; 61 | 62 | static int running = 0; 63 | 64 | // Local Functions 65 | static int __calibrate_sensors(void); 66 | static int __sensor_setup(void); 67 | static int __kalman_setup(void); 68 | static void __state_estimate(void); 69 | 70 | /** 71 | * @brief interrupt handler to catch ctrl-c 72 | */ 73 | static void __signal_handler(__attribute__ ((unused)) int dummy) 74 | { 75 | running=0; 76 | return; 77 | } 78 | 79 | static int __calibrate_sensors(void) 80 | { 81 | rc_mpu_config_t config = rc_mpu_default_config(); 82 | config.i2c_bus = I2C_BUS; 83 | 84 | if(rc_mpu_calibrate_accel_routine(config)<0) 85 | { 86 | printf("Failed to complete accelerometer calibration\n"); 87 | return -1; 88 | } 89 | printf("\nacceleometer calibration file written\n"); 90 | printf("run rc_test_mpu to check performance\n"); 91 | 92 | printf("\nThis program will generate a new gyro calibration file\n"); 93 | printf("keep your board very still for this procedure.\n"); 94 | printf("Press any key to continue\n"); 95 | getchar(); 96 | printf("Starting calibration routine\n"); 97 | 98 | if(rc_mpu_calibrate_gyro_routine(config)<0) 99 | { 100 | printf("Failed to complete gyro calibration\n"); 101 | return -1; 102 | } 103 | printf("\ngyro calibration file written\n"); 104 | printf("run rc_test_mpu to check performance\n"); 105 | 106 | printf("\n"); 107 | printf("This will sample the magnetometer for the next 15 seconds\n"); 108 | printf("Rotate the board around in the air through as many orientations\n"); 109 | printf("as possible to collect sufficient data for calibration\n"); 110 | printf("Press any key to continue\n"); 111 | getchar(); 112 | printf("spin spin spin!!!\n\n"); 113 | 114 | // wait for the user to actually start 115 | rc_usleep(2000000); 116 | if(rc_mpu_calibrate_mag_routine(config)<0) 117 | { 118 | printf("Failed to complete magnetometer calibration\n"); 119 | return -1; 120 | } 121 | printf("\nmagnetometer calibration file written\n"); 122 | printf("run rc_test_mpu to check performance\n"); 123 | 124 | return 0; 125 | } 126 | 127 | /** 128 | * Setup and Configure sensors 129 | */ 130 | static int __sensor_setup(void) 131 | { 132 | conf = rc_mpu_default_config(); 133 | conf.i2c_bus = I2C_BUS; 134 | conf.gpio_interrupt_pin_chip = GPIO_INT_PIN_CHIP; 135 | conf.gpio_interrupt_pin = GPIO_INT_PIN_PIN; 136 | conf.dmp_sample_rate = SAMPLE_RATE; 137 | //conf.dmp_fetch_accel_gyro = 1; 138 | conf.enable_magnetometer = 1; 139 | 140 | printf("hellosen\n"); 141 | return 0; 142 | } 143 | 144 | /** 145 | * Allocate appropriate memory for system 146 | * Read in system matrices from Model_Parameters.h 147 | */ 148 | static int __kalman_setup(void) 149 | { 150 | rc_matrix_zeros(&F, n, n); 151 | rc_matrix_zeros(&G, n, m); 152 | rc_matrix_zeros(&H, p+2, n); 153 | rc_matrix_zeros(&Q, n, n); 154 | rc_matrix_zeros(&R, p+2, p+2); 155 | rc_matrix_zeros(&Pi, n, n); 156 | 157 | rc_vector_zeros(&r, p); 158 | rc_vector_zeros(&xe, p); 159 | rc_vector_zeros(&u, m); 160 | rc_vector_zeros(&u1, m); 161 | rc_vector_zeros(&u2, m); 162 | rc_vector_zeros(&y, p+2); 163 | rc_vector_zeros(&ye, p); 164 | 165 | rc_matrix_zeros(&K, m, n); 166 | rc_matrix_zeros(&Ki, m, p); 167 | 168 | // define system 169 | for(int i = 0; i=BMP_RATE_DIV) 242 | { 243 | // perform the i2c reads to the sensor, on bad read just try later 244 | if(rc_bmp_read(&bmp_data)) return; 245 | bmp_sample_counter=0; 246 | } 247 | return; 248 | } 249 | 250 | /** 251 | * main() serves to parse user options, initialize the imu and interrupt 252 | * handler, and wait for the rc_get_state()==EXITING condition before exiting 253 | * cleanly. The imu_interrupt function print_data() is what actually prints new 254 | * imu data to the screen after being set with rc_mpu_set_dmp_callback(). 255 | * 256 | * @return 0 on success -1 on failure 257 | */ 258 | int main() 259 | { 260 | __sensor_setup(); 261 | __kalman_setup(); 262 | 263 | // set signal handler so the loop can exit cleanly to ^C cmd 264 | signal(SIGINT, __signal_handler); 265 | running = 1; 266 | 267 | // init barometer sample at 87Hz, no internal filter 268 | if(rc_bmp_init(BMP_OVERSAMPLE_16, BMP_FILTER_16)) 269 | { 270 | printf("rc_bmp_init_failed\n"); 271 | return -1; 272 | } 273 | 274 | if(rc_bmp_read(&bmp_data)) 275 | { 276 | return -1; 277 | } 278 | 279 | printf("waiting for sensors to settle\n"); 280 | fflush(stdout); 281 | rc_usleep(10000000); 282 | 283 | // calculate altitude offset 284 | for (int i = 0; i < 10;i++) 285 | { 286 | rc_bmp_read(&bmp_data); 287 | innit_alt += bmp_data.alt_m; 288 | rc_usleep(100000); 289 | } 290 | innit_alt = innit_alt/10; 291 | printf("helloinit:%5.2f \n",innit_alt); 292 | 293 | // set up the imu for dmp interrupt operation 294 | if(rc_mpu_initialize_dmp(&mpu_data, conf)) 295 | { 296 | printf("rc_mpu_initialize_failed\n"); 297 | return -1; 298 | } 299 | 300 | // set up the kalman filter 301 | if(rc_kalman_alloc_lin(&kf,F,G,H,Q,R,Pi)) 302 | { 303 | printf("rc_kalman_alloc_lin_failed\n"); 304 | return -1; 305 | } 306 | 307 | // Interrupt function to call state estimator. 308 | rc_mpu_set_dmp_callback(&__state_estimate); 309 | 310 | // main program loop 311 | while(running) 312 | { 313 | /* 314 | ye.d[0] = r.d[0] - kf.x_est.d[0]; 315 | ye.d[1] = r.d[0] - kf.x_est.d[3]; 316 | ye.d[2] = r.d[0] - kf.x_est.d[5]; 317 | ye.d[3] = r.d[0] - kf.x_est.d[7]; 318 | 319 | xe.d[0] = xe.d[0] + T*ye.d[0]; 320 | xe.d[1] = xe.d[1] + T*ye.d[1]; 321 | xe.d[2] = xe.d[2] + T*ye.d[2]; 322 | xe.d[3] = xe.d[3] + T*ye.d[3]; 323 | 324 | rc_matrix_times_col_vec(K,kf.x_est,&u1); 325 | rc_matrix_times_col_vec(Ki,xe,&u2); 326 | 327 | u.d[0] = u1.d[0] + u2.d[0] + u_e[0]; 328 | u.d[1] = u1.d[1] + u2.d[1] + u_e[1]; 329 | u.d[2] = u1.d[2] + u2.d[2] + u_e[2]; 330 | u.d[3] = u1.d[3] + u2.d[3] + u_e[3]; 331 | u.d[4] = u1.d[4] + u2.d[4] + u_e[4]; 332 | u.d[5] = u1.d[5] + u2.d[5] + u_e[5]; 333 | 334 | rc_vector_print(u); 335 | */ 336 | rc_usleep(10000); // run at 100Hz 337 | } 338 | 339 | // shut things down 340 | rc_bmp_power_off(); 341 | rc_mpu_power_off(); 342 | rc_kalman_free(&kf); 343 | printf("\n"); 344 | fflush(stdout); 345 | 346 | return 0; 347 | } 348 | -------------------------------------------------------------------------------- /Flight Control Software/Makefile: -------------------------------------------------------------------------------- 1 | # This is a general use makefile for librobotcontrol projects written in C. 2 | # Just change the target name to match your main source code filename. 3 | 4 | TARGET = LQG_Controller 5 | 6 | # compiler and linker binaries 7 | CC := gcc 8 | LINKER := gcc 9 | 10 | # compiler and linker flags 11 | WFLAGS := -Wall -Wextra -Werror=float-equal -Wuninitialized -Wunused-variable -Wdouble-promotion 12 | CFLAGS := -g -c -Wall 13 | LDFLAGS := -pthread -lm -lrt -l:librobotcontrol.so.1 14 | 15 | SOURCES := $(wildcard *.c) 16 | INCLUDES := $(wildcard *.h) 17 | OBJECTS := $(SOURCES:$%.c=$%.o) 18 | 19 | prefix := /usr/local 20 | RM := rm -f 21 | INSTALL := install -m 4755 22 | INSTALLDIR := install -d -m 755 23 | 24 | SYMLINK := ln -s -f 25 | SYMLINKDIR := /etc/robotcontrol 26 | SYMLINKNAME := link_to_startup_program 27 | 28 | 29 | # linking Objects 30 | $(TARGET): $(OBJECTS) 31 | @$(LINKER) -o $@ $(OBJECTS) $(LDFLAGS) 32 | @echo "Made: $@" 33 | 34 | 35 | # compiling command 36 | $(OBJECTS): %.o : %.c $(INCLUDES) 37 | @$(CC) $(CFLAGS) $(WFLAGS) $(DEBUGFLAG) $< -o $@ 38 | @echo "Compiled: $@" 39 | 40 | all: $(TARGET) 41 | 42 | debug: 43 | $(MAKE) $(MAKEFILE) DEBUGFLAG="-g -D DEBUG" 44 | @echo " " 45 | @echo "$(TARGET) Make Debug Complete" 46 | @echo " " 47 | 48 | install: 49 | @$(MAKE) --no-print-directory 50 | @$(INSTALLDIR) $(DESTDIR)$(prefix)/bin 51 | @$(INSTALL) $(TARGET) $(DESTDIR)$(prefix)/bin 52 | @echo "$(TARGET) Install Complete" 53 | 54 | clean: 55 | @$(RM) $(OBJECTS) 56 | @$(RM) $(TARGET) 57 | @echo "$(TARGET) Clean Complete" 58 | 59 | uninstall: 60 | @$(RM) $(DESTDIR)$(prefix)/bin/$(TARGET) 61 | @echo "$(TARGET) Uninstall Complete" 62 | 63 | runonboot: 64 | @$(MAKE) install --no-print-directory 65 | @$(SYMLINK) $(DESTDIR)$(prefix)/bin/$(TARGET) $(SYMLINKDIR)/$(SYMLINKNAME) 66 | @echo "$(TARGET) Set to Run on Boot" -------------------------------------------------------------------------------- /Flight Control Software/Model_Parameters.h: -------------------------------------------------------------------------------- 1 | #ifndef __MODEL_PARAMETERS_H__ 2 | #define __MODEL_PARAMETERS_H__ 3 | 4 | // System Dimensions 5 | int n = 14; 6 | int m = 6; 7 | int p = 4; 8 | 9 | double T = 0.01; 10 | 11 | double w1 = 2.0529e+03; 12 | double w4 = 2.0704e+03; 13 | 14 | double u_e[6] = {176.1000,178.5000,177.2000,177.6000,202.2000,204.5000}; 15 | 16 | // System parameters 17 | double A[14][14] = {{1, 0.01, 0, 0, 0, 0, 0, 0, 5.756e-08, 5.756e-08, 5.756e-08, 5.7562e-08, 5.7562e-08, 5.7562e-08}, 18 | {0, 1, 0, 0, 0, 0, 0, 0, 1.072e-05, 1.072e-05, 1.072e-05, 1.0724e-05, 1.0724e-05, 1.0724e-05}, 19 | {0, 0, 1, 0.01, 0, 0, 0, 0, 0.0001152, 0.000115, -0.000115, -0.0001, 0, 0}, 20 | {0, 0, 0, 1, 0, 0, 0, 0, 0.021458, 0.0214, -0.021458, -0.0214, 0, 0}, 21 | {0, 0, 0, 0, 1, 0.01, 0, 0, -4.0162e-06, -4.016e-06, -4.016227e-06, -4.0162e-06, 8.0324e-06, 8.03245e-06}, 22 | {0, 0, 0, 0, 0, 1, 0, 0, -0.000748, -0.000748, -0.000748, -0.000748, 0.0015, 0.00149}, 23 | {0, 0, 0, 0, 0, 0, 1, 0.01, -9.6098e-08, 9.60983e-08, 9.609833e-08, -9.6098e-08, -9.6098e-08, 9.60983e-08}, 24 | {0, 0, 0, 0, 0, 0, 0, 1, -1.79e-05, 1.7904e-05, 1.790439e-05, -1.7904e-05, -1.7904e-05, 1.7904e-05}, 25 | {0, 0, 0, 0, 0, 0, 0, 0, 0.6426, 0, 0, 0, 0, 0}, 26 | {0, 0, 0, 0, 0, 0, 0, 0, 0, 0.6426, 0, 0, 0, 0}, 27 | {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.6426, 0, 0, 0}, 28 | {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.6426, 0, 0}, 29 | {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.6426, 0}, 30 | {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.6426}}; 31 | 32 | double B[14][6] = {{1.024e-07, 1.02444e-07, 1.02445e-07, 1.02444e-07, 1.02444e-07, 1.024e-07}, 33 | {2.967e-05, 2.9673e-05, 2.96732e-05, 2.9673e-05, 2.967e-05, 2.967e-05}, 34 | {0.0002049, 0.000205, -0.000205, -0.0002, 0, 0}, 35 | {0.05937, 0.05937, -0.059, -0.0593, 0, 0}, 36 | {-7.14778e-06,-7.14778e-06, -7.147e-06, -7.1477e-06, 1.42955e-05, 1.429e-05}, 37 | {-0.00207, -0.00207, -0.00207, -0.00207, 0.00414, 0.0041}, 38 | {-1.7103e-07, 1.7103e-07, 1.7103e-07, -1.71028e-07, -1.71028e-07, 1.7102e-07}, 39 | {-4.9539e-05, 4.954e-05, 4.9539e-05, -4.953e-05, -4.953e-05, 4.95e-05}, 40 | { 4.16618, 0, 0, 0, 0, 0}, 41 | { 0, 4.166, 0, 0, 0, 0}, 42 | { 0, 0, 4.1661, 0, 0, 0}, 43 | { 0, 0, 0, 4.166, 0, 0}, 44 | { 0, 0, 0, 0, 4.166, 0}, 45 | { 0, 0, 0, 0, 0, 4.166}}; 46 | 47 | double C[4+2][14] = {{1,0,0,0,0,0,0,0,0,0,0,0,0,0}, 48 | {0,0,1,0,0,0,0,0,0,0,0,0,0,0}, 49 | {0,0,0,0,1,0,0,0,0,0,0,0,0,0}, 50 | {0,0,0,0,0,0,1,0,0,0,0,0,0,0}, 51 | {0,0,0,0,0,0,0,0,1,0,0,0,0,0}, 52 | {0,0,0,0,0,0,0,0,0,0,0,1,0,0}}; 53 | 54 | double D[4][6] = {{0,0,0,0,0,0}, 55 | {0,0,0,0,0,0}, 56 | {0,0,0,0,0,0}, 57 | {0,0,0,0,0,0}}; 58 | 59 | double Rw[14][14] = {{0.5,0,0,0,0,0,0,0,0,0,0,0,0,0}, 60 | {0,0.5,0,0,0,0,0,0,0,0,0,0,0,0}, 61 | {0,0,0.01,0,0,0,0,0,0,0,0,0,0,0}, 62 | {0,0,0,0.1,0,0,0,0,0,0,0,0,0,0}, 63 | {0,0,0,0,0.01,0,0,0,0,0,0,0,0,0}, 64 | {0,0,0,0,0,0.01,0,0,0,0,0,0,0,0}, 65 | {0,0,0,0,0,0,0.01,0,0,0,0,0,0,0}, 66 | {0,0,0,0,0,0,0,0.01,0,0,0,0,0,0}, 67 | {0,0,0,0,0,0,0,0,1^(-9),0,0,0,0,0}, 68 | {0,0,0,0,0,0,0,0,0,1^(-9),0,0,0,0}, 69 | {0,0,0,0,0,0,0,0,0,0,1^(-9),0,0,0}, 70 | {0,0,0,0,0,0,0,0,0,0,0,1^(-9),0,0}, 71 | {0,0,0,0,0,0,0,0,0,0,0,0,1^(-9),0}, 72 | {0,0,0,0,0,0,0,0,0,0,0,0,0,1^(-9)}}; 73 | 74 | double Rv[4+2][4+2] = {{500,0,0,0,0,0}, 75 | {0,0.00001,0,0,0,0}, 76 | {0,0,0.00001,0,0,0}, 77 | {0,0,0,0.00001,0,0}, 78 | {0,0,0,0,0.00001,0}, 79 | {0,0,0,0,0,0.00001}}; 80 | 81 | double Kdt[6][14] = {{2.5172,0.7145,4.9839,0.2234,-3.2353,-0.1557,-1.8581,-1.0421,0,0,0,0,0,0}, 82 | {2.1882,0.6233,4.2999,0.1939,-2.7826,-0.1330,2.1449,1.2041,0,0,0,0,0,0}, 83 | {2.5451,0.7246,-5.0106,-0.2245,-3.2439,-0.1552,1.8345,1.0299,0,0,0,0,0,0}, 84 | {2.1489,0.6096,-4.2745,-0.1926,-2.7685,-0.1334,-2.1641,-1.2139,0,0,0,0,0,0}, 85 | {2.6495,0.7514,-0.0057,-0.0009,6.0841,0.2876,-2.1276,-1.1933,0,0,0,0,0,0}, 86 | {2.2999,0.6548,0.0019,0.0008,5.2360,0.2486,2.4624,1.3823,0,0,0,0,0,0}}; 87 | 88 | double Kidt[6][4] = {{0,0,0,0}, 89 | {0,0,0,0}, 90 | {0,0,0,0}, 91 | {0,0,0,0}, 92 | {0,0,0,0}, 93 | {0,0,0,0}}; 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /LQG-14States/ControlDesignLQG14States.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Mass of the Multirotor in Kilograms as taken from the CAD 7 | 8 | M = 1.455882; 9 | g = 9.81; 10 | 11 | %% Dimensions of Multirotor 12 | 13 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 14 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 15 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 16 | 17 | %% Mass Moment of Inertia as Taken from the CAD 18 | 19 | Ixx = 0.014; 20 | Iyy = 0.028; 21 | Izz = 0.038; 22 | 23 | %% Motor Thrust and Torque Constants (To be determined experimentally) 24 | 25 | Kw = 0.85; 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 | 34 | Dxx = 0.01212; 35 | Dyy = 0.01212; 36 | Dzz = 0.0648; 37 | 38 | %% Equilibrium Input 39 | 40 | %W_e = sqrt(((M*g)/(3*(Kthrust+(Kw*Kthrust))))); 41 | %W_e = ((-6*Kthrust2) + sqrt((6*Kthrust2)^2 - (4*(-M*g)*(3*Kw*Kthrust + 3*Kthrust))))/(2*(3*Kw*Kthrust + 3*Kthrust)); 42 | %U_e = (W_e/Ku); 43 | U_e = [176.1,178.5,177.2,177.6,202.2,204.5]'; 44 | W_e = U_e*Ku; 45 | W_eV = [0;0;0;0;0;0;0;0;W_e]; 46 | 47 | %% Define Discrete-Time BeagleBone Dynamics 48 | 49 | T = 0.01; % Sample period (s)- 100Hz 50 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 51 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 52 | 53 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 54 | 55 | % A = 14x14 matrix 56 | A = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 57 | 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; 58 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 59 | 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; 60 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0; 61 | 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; 62 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0; 63 | 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; 64 | 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0, 0, 0; 65 | 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0, 0; 66 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0; 67 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0; 68 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0; 69 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau]; 70 | 71 | % B = 14x6 matrix 72 | B = [0, 0, 0, 0, 0, 0; 73 | 0, 0, 0, 0, 0, 0; 74 | 0, 0, 0, 0, 0, 0; 75 | 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 | Ku/Mtau, 0, 0, 0, 0, 0; 81 | 0, Ku/Mtau, 0, 0, 0, 0; 82 | 0, 0, Ku/Mtau, 0, 0, 0; 83 | 0, 0, 0, Ku/Mtau, 0, 0; 84 | 0, 0, 0, 0, Ku/Mtau, 0; 85 | 0, 0, 0, 0, 0, Ku/Mtau]; 86 | 87 | % C = 4x14 matrix 88 | C = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 89 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 90 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 91 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]; 92 | 93 | % D = 4x6 matrix 94 | D = 0; 95 | 96 | %% Discrete-Time System 97 | 98 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 99 | 100 | Adt = sysdt.a; 101 | Bdt = sysdt.b; 102 | Cdt = sysdt.c; 103 | Ddt = sysdt.d; 104 | 105 | %% System Characteristics 106 | 107 | poles = eig(Adt); 108 | Jpoles = jordan(Adt); 109 | % System Unstable 110 | 111 | figure(1) 112 | plot(poles,'*'); 113 | grid on 114 | title('Discrete System Eigenvalues'); 115 | 116 | cntr = rank(ctrb(Adt,Bdt)); 117 | % Fully Reachable 118 | 119 | obs = rank(obsv(Adt,Cdt)); 120 | % Partially Observable but Detectable 121 | 122 | %% Discrete-Time Full Integral Augmaneted System 123 | 124 | Cr = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 125 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 126 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 127 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]; 128 | 129 | r = 4; % number of reference inputs 130 | n = size(A,2); % number of states 131 | q = size(Cr,1); % number of controlled outputs 132 | 133 | Dr = zeros(q,6); 134 | 135 | Adtaug = [Adt zeros(n,r); 136 | -Cr*Adt eye(q,r)]; 137 | 138 | Bdtaug = [Bdt; -Cr*Bdt]; 139 | 140 | Cdtaug = [C zeros(r,r)]; 141 | 142 | %% Discrete-Time Full State-Feedback Control 143 | % State feedback control design with integral control via state augmentation 144 | % Z Phi Theta Psi are controlled outputs 145 | 146 | Q = diag([200,500,1000,10,1000,15,1000,10,0,0,0,0,0,0,1,35,35,0.1]); % State penalty 147 | R = (1*10^-3)*eye(6,6); % Control penalty 148 | 149 | Kdtaug = dlqr(Adtaug,Bdtaug,Q,R,0); % DT State-Feedback Controller Gains 150 | Kdt = Kdtaug(:,1:n); % LQR Gains 151 | Kidt = Kdtaug(:,n+1:end); % Integral Gains 152 | 153 | Kxr = zeros(14,4); 154 | Kxr(1,1) = 1; 155 | Kxr(3,2) = 1; 156 | Kxr(5,3) = 1; 157 | Kxr(7,4) = 1; 158 | 159 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 160 | 161 | Cy = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 162 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 163 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 164 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0; 165 | 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0; 166 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0]; 167 | 168 | Dy = zeros(6,6); 169 | 170 | Gdt = 1e-1*eye(n); 171 | Hdt = zeros(size(Cy,1),size(Gdt,2)); % No process noise on measurements 172 | 173 | Rw = diag([0.001,1,0.00001,1,0.00001,1,0.00001,1,10^-10,10^-10,10^-10,10^-10,10^-10,10^-10]); % Process noise covariance matrix 174 | Rv = diag([1,1,1,1,1,1])*10^-5; % Measurement noise covariance matrix Note: use low gausian noice for Rv 175 | 176 | sys4kf = ss(Adt,[Bdt Gdt],Cy,[Dy Hdt],T); 177 | 178 | [kdfilt,Ldt] = kalman(sys4kf,Rw,Rv); 179 | 180 | %% Dynamic Simulation 181 | 182 | Time = 100; 183 | kT = round(Time/T); 184 | 185 | X = zeros(14,kT); 186 | Xreal = zeros(18,kT); 187 | Xe = zeros(4,kT); 188 | Y = zeros(4,kT); 189 | e = zeros(6,kT); 190 | 191 | U = ones(6,kT); 192 | U(:,1) = U_e; 193 | 194 | Ref = [0;0;0;0]; 195 | x_ini = [0;0;0;0;0;0;0;0;0;0;0;0;0;0]; 196 | 197 | X(:,2) = x_ini; 198 | Xest = X; 199 | Xest(:,1) = x_ini+0.001*randn(14,1); 200 | Xreal(5:end,2) = x_ini; 201 | 202 | %U(:,1) = 0; 203 | 204 | for k = 2:kT-1 205 | 206 | %%Reference Setting 207 | if k == 10/T 208 | Ref(1) = 1; 209 | end 210 | if k == 15/T 211 | Ref(2) = 30*pi/180; 212 | end 213 | if k == 20/T 214 | Ref(2) = 0; 215 | end 216 | if k == 25/T 217 | Ref(3) = 30*pi/180; 218 | end 219 | if k == 30/T 220 | Ref(3) = 0; 221 | end 222 | if k == 40/T 223 | Ref(4) = 90*pi/180; 224 | end 225 | 226 | %%Estimation 227 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % No KF Linear Prediction 228 | 229 | % Xest(:,k) = Xreal([5,6,7,8,9,10,11,12,13:18],k); % No KF Non Linear Prediction 230 | 231 | t_span = [0,T]; 232 | xkf = [0;0;0;0;Xest(:,k-1)]; 233 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k-1)),t_span,xkf); % Nonlinear Prediction 234 | Xest(:,k) = xode.y(5:18,end); 235 | Y(:,k) = Xreal([5,7,9,11],k); 236 | e(:,k) = [Y(:,k) - Xest([1,3,5,7],k); 0; 0]; 237 | Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 238 | 239 | % Y(:,k) = Xreal([5,7,9,11],k); 240 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Linear Prediction 241 | % e(:,k) = [Y(:,k) - Xest([1,3,5,7],k); 0; 0]; 242 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 243 | % 244 | 245 | %%Control 246 | Xe(:,k) = Xe(:,k-1) + (Ref - Xest([1,3,5,7],k)); % Integrator 247 | %U(:,k) = U_e - [Kdt,Kidt]*[Xest(:,k); Xe(:,k)]; 248 | U(:,k) = min(800, max(0, U_e - [Kdt,Kidt]*[Xest(:,k); Xe(:,k)])); 249 | %- [Ref(1);0;Ref(2);0;Ref(3);0;Ref(4);0;W_e] 250 | 251 | %Simulation 252 | t_span = [0,T]; 253 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k)),t_span,Xreal(:,k)); 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'); -------------------------------------------------------------------------------- /LQG-14States/Hex_Dynamics.m: -------------------------------------------------------------------------------- 1 | function [dX]=Hex_Dynamics(t,X,U) 2 | %% Mass of the Multirotor in Kilograms as taken from the CAD 3 | 4 | M = 1.455882; 5 | g = 9.81; 6 | 7 | %% Dimensions of Multirotor 8 | 9 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 10 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 11 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 12 | 13 | %% Mass Moment of Inertia as Taken from the CAD 14 | % Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 15 | 16 | Ixx = 0.014; 17 | Iyy = 0.028; 18 | Izz = 0.038; 19 | 20 | %% Motor Thrust and Torque Constants (To be determined experimentally) 21 | 22 | Kw = 0.85; 23 | Ktau = 7.708e-10; 24 | Kthrust = 1.812e-07; 25 | Kthrust2 = 0.0007326; 26 | Mtau = (1/44.22); 27 | Ku = 515.5*Mtau; 28 | 29 | %% Air resistance damping coeeficients 30 | 31 | Dxx = 0.01212; 32 | Dyy = 0.01212; 33 | Dzz = 0.0648; 34 | 35 | %% X = [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4 w5 w6] 36 | 37 | x = X(1); 38 | xdot = X(2); 39 | y = X(3); 40 | ydot = X(4); 41 | z = X(5); 42 | zdot = X(6); 43 | 44 | phi = X(7); 45 | p = X(8); 46 | theta = X(9); 47 | q = X(10); 48 | psi = X(11); 49 | r = X(12); 50 | 51 | w1 = X(13); 52 | w2 = X(14); 53 | w3 = X(15); 54 | w4 = X(16); 55 | w5 = X(17); 56 | w6 = X(18); 57 | 58 | %% Initialise Outputs 59 | 60 | dX = zeros(18,1); 61 | Y = zeros(4,1); 62 | 63 | %% Motor Dynamics: dX = [w1dot w2dot w3dot w4dot w5dot w6dot], U = Pulse Width of the pwm signal 0-1000% 64 | 65 | dX(13) = -(1/Mtau)*w1 + Ku/Mtau*U(1); 66 | dX(14) = -(1/Mtau)*w2 + Ku/Mtau*U(2); 67 | dX(15) = -(1/Mtau)*w3 + Ku/Mtau*U(3); 68 | dX(16) = -(1/Mtau)*w4 + Ku/Mtau*U(4); 69 | dX(17) = -(1/Mtau)*w5 + Ku/Mtau*U(5); 70 | dX(18) = -(1/Mtau)*w6 + Ku/Mtau*U(6); 71 | 72 | %% Motor Forces and Torques 73 | 74 | F = zeros(6,1); 75 | T = zeros(6,1); 76 | 77 | F(1)= Kw*Kthrust*(w1^2) + Kthrust2*w1; 78 | F(2)= Kthrust*(w2^2) + Kthrust2*w2; 79 | F(3)= Kw*Kthrust*(w3^2)+ Kthrust2*w3; 80 | F(4)= Kthrust*(w4^2) + Kthrust2*w4; 81 | F(5)= Kw*Kthrust*(w5^2) + Kthrust2*w5; 82 | F(6)= Kthrust*(w6^2) + Kthrust2*w6; 83 | 84 | T(1)= -Ktau*(w1^2); 85 | T(2)= Ktau*(w2^2); 86 | T(3)= Ktau*(w3^2); 87 | T(4)= -Ktau*(w4^2); 88 | T(5)= -Ktau*(w5^2); 89 | T(6)= Ktau*(w6^2); 90 | 91 | Fn = sum(F); 92 | Tn = sum(T); 93 | 94 | %% First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 95 | 96 | dX(1) = X(2); 97 | dX(3) = X(4); 98 | dX(5) = X(6); 99 | dX(7) = p + q*(sin(phi)*tan(theta)) + r*(cos(phi)*tan(theta)); 100 | dX(9) = q*cos(phi) - r*sin(phi); 101 | dX(11) = q*(sin(phi)/cos(theta)) + r*(cos(phi)/cos(theta)); 102 | 103 | %% Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 104 | 105 | dX(2) = Fn/M*(cos(phi)*sin(theta)*cos(psi)) + Fn/M*(sin(phi)*sin(psi)) - (Dxx/M)*xdot; 106 | dX(4) = Fn/M*(cos(phi)*sin(theta)*sin(psi)) - Fn/M*(sin(phi)*cos(psi)) - (Dyy/M)*ydot; 107 | dX(6) = Fn/M*(cos(phi)*cos(theta)) - g - (Dzz/M)*zdot; 108 | 109 | dX(8) = (L1/Ixx)*((F(1)+F(2)) - (F(3)+F(4))) - ((Izz-Iyy)/Ixx)*(r*q); 110 | dX(10) = (L3/Iyy)*(F(5)+F(6)) - (L2/Iyy)*(F(1)+F(2)+F(3)+F(4)) - ((Izz-Ixx)/Iyy)*(p*r); 111 | dX(12) = Tn/Izz - ((Iyy-Ixx)/Izz)*(p*q); 112 | 113 | %% Measured States 114 | 115 | Y(1) = z; 116 | Y(2) = phi; 117 | Y(3) = theta; 118 | Y(4) = psi; 119 | 120 | end -------------------------------------------------------------------------------- /LQG-14States/Y6MultirotorLQRSimulation14State.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/LQG-14States/Y6MultirotorLQRSimulation14State.slx -------------------------------------------------------------------------------- /LQG-8States/ControlDesignLQG8States.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Mass of the Multirotor in Kilograms as taken from the CAD 7 | 8 | M = 1.455882; 9 | g = 9.81; 10 | 11 | %% Dimensions of Multirotor 12 | 13 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 14 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 15 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 16 | 17 | %% Mass Moment of Inertia as Taken from the CAD 18 | 19 | Ixx = 0.014; 20 | Iyy = 0.028; 21 | Izz = 0.038; 22 | 23 | %% Motor Thrust and Torque Constants (To be determined experimentally) 24 | 25 | Kw = 0.85; 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 | 34 | Dxx = 0.01212; 35 | Dyy = 0.01212; 36 | Dzz = 0.0648; 37 | 38 | %% Equilibrium Input 39 | 40 | %W_e = sqrt(((M*g)/(3*(Kthrust+(Kw*Kthrust))))); 41 | W_e = ((-6*Kthrust2) + sqrt((6*Kthrust2)^2 - (4*(-M*g)*(3*Kw*Kthrust + 3*Kthrust))))/(2*(3*Kw*Kthrust + 3*Kthrust)); 42 | U_e = (W_e/Ku); 43 | 44 | %% Define Discrete-Time BeagleBone Dynamics 45 | 46 | T = 0.01; % Sample period (s)- 100Hz 47 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 48 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 49 | 50 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 51 | 52 | % A = 8x8 matrix 53 | A = [0, 1, 0, 0, 0, 0, 0, 0; 54 | 0, 0, 0, 0, 0, 0, 0, 0; 55 | 0, 0, 0, 1, 0, 0, 0, 0; 56 | 0, 0, 0, 0, 0, 0, 0, 0; 57 | 0, 0, 0, 0, 0, 1, 0, 0; 58 | 0, 0, 0, 0, 0, 0, 0, 0; 59 | 0, 0, 0, 0, 0, 0, 0, 1; 60 | 0, 0, 0, 0, 0, 0, 0, 0]; 61 | 62 | % B = 8x6 matrix 63 | B = [0, 0, 0, 0, 0, 0; 64 | 2*Kthrust*W_e*((Ku))/M, 2*Kw*Kthrust*W_e*((Ku))/M, 2*Kthrust*W_e*((Ku))/M, 2*Kw*Kthrust*W_e*((Ku))/M, 2*Kthrust*W_e*((Ku))/M, 2*Kw*Kthrust*W_e*((Ku))/M; 65 | 0, 0, 0, 0, 0, 0; 66 | 2*L1*Kthrust*W_e*((Ku))/Ixx, 2*L1*Kw*Kthrust*W_e*((Ku))/Ixx, -2*L1*Kthrust*W_e*((Ku))/Ixx, -2*L1*Kw*Kthrust*W_e*((Ku))/Ixx, 0, 0; 67 | 0, 0, 0, 0, 0, 0; 68 | -2*L2*Kthrust*W_e*((Ku))/Iyy, -2*L2*Kw*Kthrust*W_e*((Ku))/Iyy, -2*L2*Kthrust*W_e*((Ku))/Iyy, -2*L2*Kw*Kthrust*W_e*((Ku))/Iyy, 2*L3*Kthrust*W_e*((Ku))/Iyy,2*L3*Kw*Kthrust*W_e*((Ku))/Iyy; 69 | 0, 0, 0, 0, 0, 0; 70 | -2*Ktau/Izz*W_e*((Ku)), 2*Ktau*W_e*((Ku))/Izz, 2*Ktau*W_e*((Ku))/Izz, -2*Ktau*W_e*((Ku))/Izz, -2*Ktau*W_e*((Ku))/Izz, 2*Ktau*W_e*((Ku))/Izz]; 71 | 72 | % C = 4x8 matrix 73 | C = [1, 0, 0, 0, 0, 0, 0, 0; 74 | 0, 0, 1, 0, 0, 0, 0, 0; 75 | 0, 0, 0, 0, 1, 0, 0, 0; 76 | 0, 0, 0, 0, 0, 0, 1, 0]; 77 | 78 | % D = 4x6 matrix 79 | D = 0; 80 | 81 | %% Discrete-Time System 82 | 83 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 84 | 85 | Adt = sysdt.a; 86 | Bdt = sysdt.b; 87 | Cdt = sysdt.c; 88 | Ddt = sysdt.d; 89 | 90 | %% System Characteristics 91 | 92 | poles = eig(Adt); 93 | Jpoles = jordan(Adt); 94 | % System Unstable 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 | 104 | Cr = [1, 0, 0, 0, 0, 0, 0, 0; 105 | 0, 0, 1, 0, 0, 0, 0, 0; 106 | 0, 0, 0, 0, 1, 0, 0, 0; 107 | 0, 0, 0, 0, 0, 0, 1, 0]; 108 | 109 | r = 4; % number of reference inputs 110 | n = size(Adt,2); % number of states 111 | q = size(Cr,1); % number of controlled outputs 112 | 113 | Dr = zeros(q,6); 114 | 115 | Adtaug = [Adt zeros(n,r); 116 | -Cr*Adt eye(q,r)]; 117 | 118 | Bdtaug = [Bdt; -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 | 126 | Q = diag([50,200,250,500,250,500,1,1000,1,30,30,0.1]); % State penalty 127 | R = (1*10^-3)*eye(6,6); % Control penalty 128 | 129 | Kdtaug = dlqr(Adtaug,Bdtaug,Q,R,0); % DT State-Feedback Controller Gains 130 | Kdt = Kdtaug(:,1:n); 131 | Kidt = -Kdtaug(:,n+1:end); 132 | 133 | Kxr = zeros(8,4); 134 | Kxr(1,1) = 1; 135 | Kxr(3,2) = 1; 136 | Kxr(5,3) = 1; 137 | Kxr(7,4) = 1; 138 | 139 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 140 | 141 | Gdt = 1e-1*eye(n); 142 | Hdt = zeros(size(C,1),size(Gdt,2)); % No process noise on measurements 143 | 144 | Rw = diag([0.000000001,1,0.000000001,1,0.000000001,1,0.000000001,1]); % Process noise covariance matrix 145 | Rv = diag([1,1,1,1])*10^-5; % Measurement noise covariance matrix Note: use low gausian noice for Rv 146 | N = zeros(size(Rw,2),size(Rv,2)); 147 | 148 | sys4kf = ss(Adt,[Bdt Gdt],Cdt,[Ddt Hdt],T); 149 | 150 | [kdfilt,Ldt] = kalman(sys4kf,Rw,Rv); 151 | -------------------------------------------------------------------------------- /LQG-8States/Hex_Dynamics.m: -------------------------------------------------------------------------------- 1 | function [dX]=Hex_Dynamics(t,X,U) 2 | %% Mass of the Multirotor in Kilograms as taken from the CAD 3 | 4 | M = 1.455882; 5 | g = 9.81; 6 | 7 | %% Dimensions of Multirotor 8 | 9 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 10 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 11 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 12 | 13 | %% Mass Moment of Inertia as Taken from the CAD 14 | % Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 15 | 16 | Ixx = 0.014; 17 | Iyy = 0.028; 18 | Izz = 0.038; 19 | 20 | %% Motor Thrust and Torque Constants (To be determined experimentally) 21 | 22 | Kw = 0.85; 23 | Ktau = 7.708e-10; 24 | Kthrust = 1.812e-07; 25 | Kthrust2 = 0.0007326; 26 | Mtau = (1/44.22); 27 | Ku = 515.5*Mtau; 28 | 29 | %% Air resistance damping coeeficients 30 | 31 | Dxx = 0.01212; 32 | Dyy = 0.01212; 33 | Dzz = 0.0648; 34 | 35 | %% X = [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4 w5 w6] 36 | 37 | x = X(1); 38 | xdot = X(2); 39 | y = X(3); 40 | ydot = X(4); 41 | z = X(5); 42 | zdot = X(6); 43 | 44 | phi = X(7); 45 | p = X(8); 46 | theta = X(9); 47 | q = X(10); 48 | psi = X(11); 49 | r = X(12); 50 | 51 | w1 = X(13); 52 | w2 = X(14); 53 | w3 = X(15); 54 | w4 = X(16); 55 | w5 = X(17); 56 | w6 = X(18); 57 | 58 | %% Initialise Outputs 59 | 60 | dX = zeros(18,1); 61 | Y = zeros(4,1); 62 | 63 | %% Motor Dynamics: dX = [w1dot w2dot w3dot w4dot w5dot w6dot], U = Pulse Width of the pwm signal 0-1000% 64 | 65 | dX(13) = -(1/Mtau)*w1 + Ku/Mtau*U(1); 66 | dX(14) = -(1/Mtau)*w2 + Ku/Mtau*U(2); 67 | dX(15) = -(1/Mtau)*w3 + Ku/Mtau*U(3); 68 | dX(16) = -(1/Mtau)*w4 + Ku/Mtau*U(4); 69 | dX(17) = -(1/Mtau)*w5 + Ku/Mtau*U(5); 70 | dX(18) = -(1/Mtau)*w6 + Ku/Mtau*U(6); 71 | 72 | %% Motor Forces and Torques 73 | 74 | F = zeros(6,1); 75 | T = zeros(6,1); 76 | 77 | F(1)= Kw*Kthrust*(w1^2) + Kthrust2*w1; 78 | F(2)= Kthrust*(w2^2) + Kthrust2*w2; 79 | F(3)= Kw*Kthrust*(w3^2)+ Kthrust2*w3; 80 | F(4)= Kthrust*(w4^2) + Kthrust2*w4; 81 | F(5)= Kw*Kthrust*(w5^2) + Kthrust2*w5; 82 | F(6)= Kthrust*(w6^2) + Kthrust2*w6; 83 | 84 | T(1)= -Ktau*(w1^2); 85 | T(2)= Ktau*(w2^2); 86 | T(3)= Ktau*(w3^2); 87 | T(4)= -Ktau*(w4^2); 88 | T(5)= -Ktau*(w5^2); 89 | T(6)= Ktau*(w6^2); 90 | 91 | Fn = sum(F); 92 | Tn = sum(T); 93 | 94 | %% First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 95 | 96 | dX(1) = X(2); 97 | dX(3) = X(4); 98 | dX(5) = X(6); 99 | dX(7) = p + q*(sin(phi)*tan(theta)) + r*(cos(phi)*tan(theta)); 100 | dX(9) = q*cos(phi) - r*sin(phi); 101 | dX(11) = q*(sin(phi)/cos(theta)) + r*(cos(phi)/cos(theta)); 102 | 103 | %% Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 104 | 105 | dX(2) = Fn/M*(cos(phi)*sin(theta)*cos(psi)) + Fn/M*(sin(phi)*sin(psi)) - (Dxx/M)*xdot; 106 | dX(4) = Fn/M*(cos(phi)*sin(theta)*sin(psi)) - Fn/M*(sin(phi)*cos(psi)) - (Dyy/M)*ydot; 107 | dX(6) = Fn/M*(cos(phi)*cos(theta)) - g - (Dzz/M)*zdot; 108 | 109 | dX(8) = (L1/Ixx)*((F(1)+F(2)) - (F(3)+F(4))) - ((Izz-Iyy)/Ixx)*(r*q); 110 | dX(10) = (L3/Iyy)*(F(5)+F(6)) - (L2/Iyy)*(F(1)+F(2)+F(3)+F(4)) - ((Izz-Ixx)/Iyy)*(p*r); 111 | dX(12) = Tn/Izz - ((Iyy-Ixx)/Izz)*(p*q); 112 | 113 | %% Measured States 114 | 115 | Y(1) = z; 116 | Y(2) = phi; 117 | Y(3) = theta; 118 | Y(4) = psi; 119 | 120 | end -------------------------------------------------------------------------------- /LQG-8States/Y6MultirotorLQGSimulation8State.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/LQG-8States/Y6MultirotorLQGSimulation8State.slx -------------------------------------------------------------------------------- /MPC-14States/ControlDesignMPC14States.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Mass of the Multirotor in Kilograms as taken from the CAD 7 | 8 | M = 1.455882; 9 | g = 9.81; 10 | 11 | %% Dimensions of Multirotor 12 | 13 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 14 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 15 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 16 | 17 | %% Mass Moment of Inertia as Taken from the CAD 18 | 19 | Ixx = 0.014; 20 | Iyy = 0.028; 21 | Izz = 0.038; 22 | 23 | %% Motor Thrust and Torque Constants (To be determined experimentally) 24 | 25 | Kw = 0.85; 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 | 34 | Dxx = 0.01212; 35 | Dyy = 0.01212; 36 | Dzz = 0.0648; 37 | 38 | %% Equilibrium Input 39 | 40 | %W_e = sqrt(((M*g)/(3*(Kthrust+(Kw*Kthrust))))); 41 | %W_e = ((-6*Kthrust2) + sqrt((6*Kthrust2)^2 - (4*(-M*g)*(3*Kw*Kthrust + 3*Kthrust))))/(2*(3*Kw*Kthrust + 3*Kthrust)); 42 | %U_e = (W_e/Ku); 43 | U_e = [176.1,178.5,177.2,177.6,202.2,204.5]'; 44 | W_e = U_e*Ku; 45 | 46 | %% Define Discrete-Time BeagleBone Dynamics 47 | 48 | T = 0.01; % Sample period (s)- 100Hz 49 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 50 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 51 | 52 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 53 | 54 | % A = 14x14 matrix 55 | A = [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 56 | 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; 57 | 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 58 | 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; 59 | 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0; 60 | 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; 61 | 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0; 62 | 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; 63 | 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0, 0, 0; 64 | 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0, 0; 65 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0, 0; 66 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0, 0; 67 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau, 0; 68 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/Mtau]; 69 | 70 | % B = 14x6 matrix 71 | B = [0, 0, 0, 0, 0, 0; 72 | 0, 0, 0, 0, 0, 0; 73 | 0, 0, 0, 0, 0, 0; 74 | 0, 0, 0, 0, 0, 0; 75 | 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 | Ku/Mtau, 0, 0, 0, 0, 0; 80 | 0, Ku/Mtau, 0, 0, 0, 0; 81 | 0, 0, Ku/Mtau, 0, 0, 0; 82 | 0, 0, 0, Ku/Mtau, 0, 0; 83 | 0, 0, 0, 0, Ku/Mtau, 0; 84 | 0, 0, 0, 0, 0, Ku/Mtau]; 85 | 86 | % C = 4x14 matrix 87 | C = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 88 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 89 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 90 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]; 91 | 92 | % D = 4x6 matrix 93 | D = 0; 94 | 95 | %% Discrete-Time System 96 | 97 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 98 | 99 | Adt = sysdt.a; 100 | Bdt = sysdt.b; 101 | Cdt = sysdt.c; 102 | Ddt = sysdt.d; 103 | 104 | r = 4; % number of reference inputs 105 | n = size(Adt,2); % number of states 106 | q = size(Cdt,1); % number of controlled outputs 107 | 108 | %% Discrete-Time Full State-Feedback Control 109 | % State feedback control, Z Phi Theta Psi are controlled outputs 110 | 111 | Q = diag([500,0,1000,0,1000,0,1000,0,0,0,0,0,0,0]); % State penalty 112 | R = (1*10^-3)*eye(6,6); % Control penalty 113 | 114 | Kdt = dlqr(Adt,Bdt,Q,R,0); % DT State-Feedback Controller Gains 115 | 116 | %% LQR Dynamic Simulation 117 | 118 | Time = 10; 119 | kT = round(Time/T); 120 | 121 | X = zeros(14,kT); 122 | U = zeros(6,kT); 123 | 124 | X(:,1) = [0;0;0;0;0;0;0;0;0;0;0;0;0;0]; 125 | 126 | for k = 1:kT-1 127 | 128 | %Control 129 | U(:,k) = -Kdt*X(:,k); 130 | 131 | %Simulation 132 | X(:,k+1) = Adt*X(:,k) + Bdt*U(:,k); 133 | end 134 | 135 | %% Plots 136 | 137 | t = (0:kT-1)*T; 138 | Rad2Deg = [180/pi,180/pi,180/pi]'; 139 | 140 | figure(1); 141 | subplot(2,1,1); 142 | plot(t,X(1,:)); 143 | legend('Alt'); 144 | title('LQR Real Output Altitude'); 145 | ylabel('Meters(m)'); 146 | 147 | subplot(2,1,2); 148 | plot(t,X([3,5,6],:).*Rad2Deg); 149 | legend('\phi','\theta','\psi'); 150 | title('LQR Real Output Attitude'); 151 | ylabel('Degrees(d)'); 152 | 153 | figure(3); 154 | plot(t,U); 155 | legend('U1','U2','U3','U4','U5','U6'); 156 | title('LQR Inputs PWM Signal'); 157 | ylabel('Micro Seconds(ms)'); 158 | 159 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 160 | % Utilises a 6 output model with motor 1 and 4 velocities being used as 161 | % virtual aditiional outputs. This is due to layout coupling leading to many different 162 | % combintaions of motor velocity producing the same outcome, therefore 163 | % making the system unobservale otherwise, therefore to combat this, 164 | % the model is assumed to be perfect with reguargdes to the 2 motors and 165 | % 0 error is fedback to the KF gains for the motors 166 | 167 | Cy = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 168 | 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 169 | 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 170 | 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0; 171 | 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0; 172 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0]; 173 | 174 | Dy = zeros(6,6); 175 | 176 | Gdt = 1e-1*eye(n); 177 | Hdt = zeros(size(Cy,1),size(Gdt,2)); % No process noise on measurements 178 | 179 | Rw = diag([0.001,1,0.00001,1,0.00001,1,0.00001,1,10^-10,10^-10,10^-10,10^-10,10^-10,10^-10]); % Process noise covariance matrix 180 | Rv = diag([1,1,1,1,1,1])*10^-5; % Measurement noise covariance matrix Note: use low gausian noice for Rv 181 | 182 | sys4kf = ss(Adt,[Bdt Gdt],Cy,[Dy Hdt],T); 183 | 184 | [kdfilt,Ldt] = kalman(sys4kf,Rw,Rv); 185 | 186 | %% Terminal State Penalty 187 | 188 | Phi = (Adt - Bdt*Kdt); % Closed Loopp System 189 | S = (Q + Kdt' * R * Kdt); 190 | P = dlyap(Phi',S); % Terminal State Penalty from Lyapunov Function 191 | 192 | %% Definition of MPC Prediction, Cost and Constaint Matrices 193 | 194 | N = 100; % Prediction Horizon 195 | 196 | %[Fxcl,Gxcl,Fycl,Gycl,Fucl,Gucl] = predict_mats_cl(Phi,Bdt,Cdt,Kdt,N); % Parameter Matrices 197 | %[Sc1,Scx] = cost_mats_cl(Fxcl,Gxcl,Fycl,Gycl,Fucl,Gucl,Q,R,P); % Cost Function Matricies 198 | 199 | % [F,G] = predict_mats(Adt,Bdt,N); % State Parameter Matrices 200 | % [H,L,M] = cost_mats(F,G,Q,R,P); % State Cost Function Matricies 201 | 202 | [SX,SC,SXC,Spsi] = ompc_cost(Adt,Bdt,Cdt,Ddt,Q,R,N); 203 | 204 | % Px = [eye(14); -1*eye(14)]; 205 | % qx = [inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf; 206 | % inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf;inf]; % State Constraints 207 | % 208 | % Pxf = Px; 209 | % qxf = qx; % Terminal State Constraints 210 | % 211 | % Pu = [eye(6); -1*eye(6)]; 212 | % qu = [800*ones(6,1); zeros(6,1)]; % Input Constraints 213 | 214 | 215 | % Kxmax = [eye(14); -1*eye(14)]; 216 | % xmax = [10;10;90*pi/180;10;90*pi/180;10;90*pi/180;10;16000;16000;16000;16000;16000;16000; 217 | % 0;0;90*pi/180;0;90*pi/180;0;90*pi/180;0;0;0;0;0;0;0]; % State Constraints 218 | % 219 | % umin = zeros(6,1); 220 | % umax = 800*ones(6,1); 221 | 222 | % 223 | % [Pc, qc, Sc] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf); % Constraints As Linear Inequality 224 | 225 | %% LQ-MPC Dynamic Simulation 226 | 227 | Time = 50; 228 | kT = round(Time/T); 229 | 230 | Xreal = zeros(18,kT); 231 | Xmodel = zeros(14,kT); 232 | X = zeros(14,kT); 233 | Y = zeros(4,kT); 234 | e = zeros(6,kT); 235 | Ref = [0;0;0;0]; 236 | 237 | U = zeros(6,kT); 238 | c = zeros(6*N,kT); 239 | Cost = zeros(1,kT); 240 | J = zeros(1,kT); 241 | 242 | X(:,2) = [0;0;0;0;0;0;0;0;0;0;0;0;0;0]; 243 | Xreal(:,1) = [0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0;0]; 244 | Xest = X; 245 | 246 | for k = 2:kT-1 247 | 248 | %%Reference Setting 249 | if k == 5/T 250 | Ref(1) = 1; 251 | end 252 | if k == 10/T 253 | Ref(2) = 30*pi/180; 254 | end 255 | if k == 15/T 256 | Ref(2) = 0; 257 | end 258 | if k == 20/T 259 | Ref(3) = 30*pi/180; 260 | end 261 | if k == 25/T 262 | Ref(3) = 0; 263 | end 264 | if k == 30/T 265 | Ref(4) = 45*pi/180; 266 | end 267 | 268 | %%Estimation 269 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % No KF Linear Prediction 270 | 271 | % Xest(:,k) = Xreal([5,6,7,8,9,10,11,12,13:18],k); % No KF Non Linear Prediction 272 | 273 | t_span = [0,T]; 274 | xkf = [0;0;0;0;Xest(:,k-1)]; 275 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k-1)),t_span,xkf); % Nonlinear Prediction 276 | Xest(:,k) = xode.y(5:18,end); 277 | Y(:,k) = Xreal([5,7,9,11],k); 278 | e(:,k) = [Y(:,k) - Xest([1,3,5,7],k); 0; 0]; 279 | Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 280 | 281 | % Y(:,k) = Xreal([5,7,9,11],k); 282 | % Xest(:,k) = Adt*Xest(:,k-1) + Bdt*(U(:,k-1)-U_e); % Linear Prediction 283 | % e(:,k) = [Y(:,k) - Xest([1,3,5,7],k); 0; 0]; 284 | % Xest(:,k) = Xest(:,k) + Ldt*e(:,k); 285 | 286 | %%Control 287 | %[Cost(:,k),U(:,k),c(:,k)] = ompc_constraints(Adt,Bdt,Cdt,Ddt,N,Q,R,Q,R,(Xest(:,k) - [Ref(1);0;Ref(2);0;Ref(3);0;Ref(4);0;W_e]),umin,umax,Kxmax,xmax); 288 | 289 | [c(:,k),Cost(:,k)] = quadprog(((SC+SC')/2),SXC'*Xest(:,k)); % Pc,qc + Sc*X(:,k) Solve Quadratic program 290 | U(:,k) = -Kdt*(Xest(:,k) - [Ref(1);0;Ref(2);0;Ref(3);0;Ref(4);0;W_e]) + c(1:6,k) + U_e; 291 | J(:,k) = Xest(:,k)'*SX*Xest(:,k) + 2*c(:,k)'*SXC'*Xest(:,k) + c(:,k)'*SC*c(:,k); 292 | 293 | % [Useq,Cost(:,k)] = quadprog(H,L*X(:,k)); % ,Pc,qc + Sc*X(:,k) Solve Quadratic program 294 | % U(:,k) = Useq(1:6); 295 | 296 | %%Simulation 297 | t_span = [0,T]; 298 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k)),t_span,Xreal(:,k)); % Nonlinear Dynamics 299 | Xreal(:,k+1) = xode.y(:,end); 300 | 301 | % [dX] = Hex_Dynamics(t,Xreal(:,k),U(:,k)); % Forward Euler Integration Nonlinear Dynamics 302 | % Xreal(:,k+1) = Xreal(:,k) + T*dX; 303 | 304 | % X(:,k+1) = Adt*Xest(:,k) + Bdt*U(:,k); % Linear Dynamics 305 | % Xmodel(:,k+1) = Adt*Xmodel(:,k) + Bdt*(U(:,k)); % Model Prediction 306 | end 307 | 308 | %% Plots 309 | 310 | t = (0:kT-1)*T; 311 | Rad2Deg = [180/pi,180/pi,180/pi]'; 312 | 313 | figure(4); 314 | subplot(2,1,1); 315 | plot(t,Xreal(5,:)); 316 | legend('Alt'); 317 | title('LQ-MPC Real Output Altitude'); 318 | ylabel('Meters(m)'); 319 | 320 | subplot(2,1,2); 321 | plot(t,Xreal([7,9,11],:).*Rad2Deg); 322 | legend('\phi','\theta','\psi'); 323 | title('LQ-MPC Real Output Attitude'); 324 | ylabel('Degrees(d)'); 325 | 326 | % subplot(2,1,1); 327 | % plot(t,X(1,:)); 328 | % legend('Alt'); 329 | % title('LQ-MPC Real Output Altitude'); 330 | % ylabel('Meters(m)'); 331 | % 332 | % subplot(2,1,2); 333 | % plot(t,X([3,5,7],:).*Rad2Deg); 334 | % legend('\phi','\theta','\psi'); 335 | % title('LQ-MPC Real Output Attitude'); 336 | % ylabel('Radians(r)'); 337 | 338 | figure(5); 339 | subplot(2,1,1); 340 | plot(t,Xest(1,:)); 341 | legend('Alt_e'); 342 | title('LQ-MPC Estimated Output Altitude'); 343 | ylabel('Meters(m)'); 344 | 345 | subplot(2,1,2); 346 | plot(t,Xest([3,5,7],:).*Rad2Deg); 347 | legend('\phi_e','\theta_e','\psi_e'); 348 | title('LQ-MPC Estimated Output Attitude'); 349 | ylabel('Degrees(d)'); 350 | 351 | figure(6); 352 | subplot(2,1,1); 353 | plot(t,e(1,:)); 354 | legend('e_z'); 355 | title('Altitude prediction error'); 356 | ylabel('Error meters(m)'); 357 | 358 | subplot(2,1,2); 359 | plot(t,e([2,3,4],:).*Rad2Deg); 360 | legend('e_\phi','e_\theta','e_\psi'); 361 | title('Attitude prediction error'); 362 | ylabel('Error degrees(d)'); 363 | 364 | figure(7); 365 | plot(t,U); 366 | legend('U1','U2','U3','U4','U5','U6'); 367 | title('LQ-MPC Inputs PWM Signal'); 368 | ylabel('Micro Seconds(ms)'); 369 | 370 | figure(8); 371 | plot(t,c(1:6,:)); 372 | legend('c1','c2','c3','c4','c5','c6'); 373 | title('LQ-MPC Inputs PWM Signal peturbations'); 374 | ylabel('Micro Seconds(ms)'); 375 | 376 | figure(9); 377 | plot(t,Cost); 378 | title('LQ-MPC Cost'); 379 | 380 | figure(10); 381 | plot(t,J); 382 | title('LQ-MPC Cost2'); 383 | 384 | figure(11); 385 | plot(t,Xest' * P * Xest); 386 | title('LQ-MPC Lyaponuv'); 387 | -------------------------------------------------------------------------------- /MPC-14States/Hex_Dynamics.m: -------------------------------------------------------------------------------- 1 | function [dX]=Hex_Dynamics(t,X,U) 2 | %% Mass of the Multirotor in Kilograms as taken from the CAD 3 | 4 | M = 1.455882; 5 | g = 9.81; 6 | 7 | %% Dimensions of Multirotor 8 | 9 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 10 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 11 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 12 | 13 | %% Mass Moment of Inertia as Taken from the CAD 14 | % Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 15 | 16 | Ixx = 0.014; 17 | Iyy = 0.028; 18 | Izz = 0.038; 19 | 20 | %% Motor Thrust and Torque Constants (To be determined experimentally) 21 | 22 | Kw = 0.85; 23 | Ktau = 7.708e-10; 24 | Kthrust = 1.812e-07; 25 | Kthrust2 = 0.0007326; 26 | Mtau = (1/44.22); 27 | Ku = 515.5*Mtau; 28 | 29 | %% Air resistance damping coeeficients 30 | 31 | Dxx = 0.01212; 32 | Dyy = 0.01212; 33 | Dzz = 0.0648; 34 | 35 | %% X = [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4 w5 w6] 36 | 37 | x = X(1); 38 | xdot = X(2); 39 | y = X(3); 40 | ydot = X(4); 41 | z = X(5); 42 | zdot = X(6); 43 | 44 | phi = X(7); 45 | p = X(8); 46 | theta = X(9); 47 | q = X(10); 48 | psi = X(11); 49 | r = X(12); 50 | 51 | w1 = X(13); 52 | w2 = X(14); 53 | w3 = X(15); 54 | w4 = X(16); 55 | w5 = X(17); 56 | w6 = X(18); 57 | 58 | %% Initialise Outputs 59 | 60 | dX = zeros(18,1); 61 | Y = zeros(4,1); 62 | 63 | %% Motor Dynamics: dX = [w1dot w2dot w3dot w4dot w5dot w6dot], U = Pulse Width of the pwm signal 0-1000% 64 | 65 | dX(13) = -(1/Mtau)*w1 + Ku/Mtau*U(1); 66 | dX(14) = -(1/Mtau)*w2 + Ku/Mtau*U(2); 67 | dX(15) = -(1/Mtau)*w3 + Ku/Mtau*U(3); 68 | dX(16) = -(1/Mtau)*w4 + Ku/Mtau*U(4); 69 | dX(17) = -(1/Mtau)*w5 + Ku/Mtau*U(5); 70 | dX(18) = -(1/Mtau)*w6 + Ku/Mtau*U(6); 71 | 72 | %% Motor Forces and Torques 73 | 74 | F = zeros(6,1); 75 | T = zeros(6,1); 76 | 77 | F(1)= Kw*Kthrust*(w1^2) + Kthrust2*w1; 78 | F(2)= Kthrust*(w2^2) + Kthrust2*w2; 79 | F(3)= Kw*Kthrust*(w3^2)+ Kthrust2*w3; 80 | F(4)= Kthrust*(w4^2) + Kthrust2*w4; 81 | F(5)= Kw*Kthrust*(w5^2) + Kthrust2*w5; 82 | F(6)= Kthrust*(w6^2) + Kthrust2*w6; 83 | 84 | T(1)= -Ktau*(w1^2); 85 | T(2)= Ktau*(w2^2); 86 | T(3)= Ktau*(w3^2); 87 | T(4)= -Ktau*(w4^2); 88 | T(5)= -Ktau*(w5^2); 89 | T(6)= Ktau*(w6^2); 90 | 91 | Fn = sum(F); 92 | Tn = sum(T); 93 | 94 | %% First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 95 | 96 | dX(1) = X(2); 97 | dX(3) = X(4); 98 | dX(5) = X(6); 99 | dX(7) = p + q*(sin(phi)*tan(theta)) + r*(cos(phi)*tan(theta)); 100 | dX(9) = q*cos(phi) - r*sin(phi); 101 | dX(11) = q*(sin(phi)/cos(theta)) + r*(cos(phi)/cos(theta)); 102 | 103 | %% Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 104 | 105 | dX(2) = Fn/M*(cos(phi)*sin(theta)*cos(psi)) + Fn/M*(sin(phi)*sin(psi)) - (Dxx/M)*xdot; 106 | dX(4) = Fn/M*(cos(phi)*sin(theta)*sin(psi)) - Fn/M*(sin(phi)*cos(psi)) - (Dyy/M)*ydot; 107 | dX(6) = Fn/M*(cos(phi)*cos(theta)) - g - (Dzz/M)*zdot; 108 | 109 | dX(8) = (L1/Ixx)*((F(1)+F(2)) - (F(3)+F(4))) - ((Izz-Iyy)/Ixx)*(r*q); 110 | dX(10) = (L3/Iyy)*(F(5)+F(6)) - (L2/Iyy)*(F(1)+F(2)+F(3)+F(4)) - ((Izz-Ixx)/Iyy)*(p*r); 111 | dX(12) = Tn/Izz - ((Iyy-Ixx)/Izz)*(p*q); 112 | 113 | %% Measured States 114 | 115 | Y(1) = z; 116 | Y(2) = phi; 117 | Y(3) = theta; 118 | Y(4) = psi; 119 | 120 | end -------------------------------------------------------------------------------- /MPC-14States/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 | -------------------------------------------------------------------------------- /MPC-14States/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 | -------------------------------------------------------------------------------- /MPC-14States/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 | -------------------------------------------------------------------------------- /MPC-14States/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 | -------------------------------------------------------------------------------- /MPC-14States/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 | -------------------------------------------------------------------------------- /MPC-14States/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 9 | #include 10 | #include 11 | 12 | #include "HX711.h" 13 | 14 | #define INTERRUPT_PIN 0 // Arduino MEGA/UNO/NANO digital pin 2 15 | 16 | volatile int interruptCount; 17 | 18 | float rpm = 0; 19 | double force; 20 | double Torque; 21 | 22 | float numpoles = 14; //14 poles for A2212/13T 23 | double timein; 24 | double timeout; 25 | double timer = 0; 26 | int critical_rpm; 27 | 28 | // HX711 circuit wiring 29 | const int LOADCELL_DOUT_PIN = 7; 30 | const int LOADCELL_SCK_PIN = 3; 31 | 32 | Servo Motor1,Motor2; 33 | File ForcedataFile; 34 | File RPMdataFile; 35 | HX711 scale; 36 | 37 | void setup() 38 | { 39 | Serial.begin(9600); 40 | 41 | Motor1.attach(5); 42 | Motor2.attach(6); 43 | Motor1.writeMicroseconds(1000); 44 | Motor2.writeMicroseconds(1000); 45 | 46 | pinMode(8, INPUT); 47 | pinMode(9, INPUT); 48 | pinMode(10, INPUT); 49 | 50 | scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN); 51 | setScales(); 52 | //scale.set_scale(801.9); // this value is obtained by calibrating the scale with known weights; see the README for details 53 | //scale.tare(); // reset the scale to 0 54 | 55 | while (!Serial){;} 56 | 57 | Serial.print("Initializing SD card..."); 58 | if (!SD.begin(4)) 59 | { 60 | Serial.println("initialization failed!"); 61 | while (1); 62 | } 63 | Serial.println("initialization done."); 64 | 65 | // RPM COUNTER INIT 66 | pinMode(INTERRUPT_PIN, INPUT); 67 | attachInterrupt(INTERRUPT_PIN, interruptFired, CHANGE); 68 | 69 | Motor1.writeMicroseconds(1000); 70 | Motor2.writeMicroseconds(1000); 71 | delay(4000); 72 | } 73 | 74 | int state = 0; 75 | void loop() 76 | { 77 | if (state == 0) 78 | { 79 | Serial.println("Idle State"); 80 | if(digitalRead(8) == 1) 81 | { 82 | state = 1; 83 | delay(1000); 84 | } 85 | //if(digitalRead(8) == 1) 86 | //{ 87 | //state = 2; 88 | //delay(1000); 89 | //} 90 | if(digitalRead(9) == 1) 91 | { 92 | state = 3; 93 | delay(1000); 94 | } 95 | } 96 | 97 | if(state == 1) 98 | { 99 | Serial.println("RPM State"); 100 | 101 | Motor1.writeMicroseconds(1000); 102 | Motor2.writeMicroseconds(1000); 103 | 104 | RPMdataFile = SD.open("RPMdata.txt", FILE_WRITE); 105 | 106 | delay(5000); 107 | Motor1.writeMicroseconds(1300); 108 | delay(5000); 109 | 110 | while(1) 111 | { 112 | timein = millis(); 113 | Motor1.writeMicroseconds(1500); 114 | 115 | checkRPM(); 116 | 117 | Serial.print("Time: "); 118 | Serial.print((timer*timeout)/1000); 119 | Serial.print("\t"); 120 | Serial.print("sampletime in ms: "); 121 | Serial.println(timeout); 122 | 123 | if(((timer*timeout)/1000) > 10) 124 | { 125 | state = 0; 126 | RPMdataFile.close(); 127 | timer = 0; 128 | Motor1.writeMicroseconds(1000); 129 | delay(1000); 130 | break; 131 | } 132 | 133 | if(digitalRead(8) == 1 || digitalRead(9) == 1) 134 | { 135 | Motor1.writeMicroseconds(1000); 136 | state = 0; 137 | delay(1000); 138 | break; 139 | } 140 | 141 | timeout = millis() - timein; 142 | timer++; 143 | } 144 | } 145 | 146 | if(state == 2) 147 | { 148 | Serial.println("Force State"); 149 | RPMdataFile = SD.open("RPMdata.txt", FILE_WRITE); 150 | ForcedataFile = SD.open("forcedata.txt", FILE_WRITE); 151 | 152 | delay(5000); 153 | Motor1.writeMicroseconds(1300); 154 | delay(5000); 155 | 156 | while(1) 157 | { 158 | timein = millis(); 159 | Motor1.writeMicroseconds(1300+((timer*timeout)/100)); 160 | checkForce(); 161 | checkRPM(); 162 | Serial.print("Time: "); 163 | Serial.print((timer*timeout)/1000); 164 | Serial.print("\t"); 165 | Serial.print("sampletime in ms: "); 166 | Serial.println(timeout); 167 | 168 | if(((timer*timeout)/1000) > 70) 169 | { 170 | state = 0; 171 | RPMdataFile.close(); 172 | timer = 0; 173 | Motor1.writeMicroseconds(1000); 174 | delay(1000); 175 | break; 176 | } 177 | 178 | if(digitalRead(8) == 1 || digitalRead(9) == 1) 179 | { 180 | Motor1.writeMicroseconds(1000); 181 | state = 0; 182 | delay(1000); 183 | break; 184 | } 185 | 186 | timeout = millis() - timein; 187 | timer++; 188 | } 189 | } 190 | 191 | if(state == 3) 192 | { 193 | Serial.println("Torque State"); 194 | RPMdataFile = SD.open("RPMdata.txt", FILE_WRITE); 195 | ForcedataFile = SD.open("forcedata.txt", FILE_WRITE); 196 | 197 | delay(5000); 198 | Motor1.writeMicroseconds(1200); 199 | delay(5000); 200 | 201 | while(1) 202 | { 203 | timein = millis(); 204 | Motor1.writeMicroseconds(1200+((timer*timeout)/100)); 205 | checkTorque(); 206 | checkRPM(); 207 | Serial.print("Time: "); 208 | Serial.print((timer*timeout)/1000); 209 | Serial.print("\t"); 210 | Serial.print("sampletime in ms: "); 211 | Serial.println(timeout); 212 | 213 | if(((timer*timeout)/1000) > 50) 214 | { 215 | state = 0; 216 | RPMdataFile.close(); 217 | timer = 0; 218 | Motor1.writeMicroseconds(1000); 219 | delay(1000); 220 | break; 221 | } 222 | 223 | if(digitalRead(8) == 1 || digitalRead(9) == 1) 224 | { 225 | Motor1.writeMicroseconds(1000); 226 | state = 0; 227 | delay(1000); 228 | break; 229 | } 230 | 231 | timeout = millis() - timein; 232 | timer++; 233 | } 234 | } 235 | } 236 | 237 | void interruptFired() 238 | { 239 | interruptCount++; 240 | } 241 | 242 | // Check RPM Function 243 | void checkRPM() 244 | { 245 | noInterrupts() ; 246 | interruptCount = 0; // set variable in critical section 247 | interrupts() ; 248 | delay(100); 249 | noInterrupts() ; 250 | critical_rpm = interruptCount ; // read variable in critical section 251 | interrupts() ; 252 | rpm = ((critical_rpm)*(60))/(numpoles)*10; 253 | Serial.print("Motor RPM: "); 254 | Serial.print(rpm); 255 | Serial.print("\t"); 256 | } 257 | 258 | // Check Force Function 259 | void checkForce() 260 | { 261 | force = (scale.get_units(1)*9.81)/1000; 262 | Serial.print("Motor Force: "); 263 | Serial.print(force); 264 | Serial.print("\t"); 265 | Serial.print((timer*timeout)/100); 266 | Serial.print("\t"); 267 | 268 | if (RPMdataFile) 269 | { 270 | RPMdataFile.print((timer*timeout)/1000); 271 | RPMdataFile.print(","); 272 | RPMdataFile.print(rpm); 273 | RPMdataFile.print(","); 274 | RPMdataFile.println(force); 275 | } 276 | else 277 | { 278 | // if the file didn't open, print an error: 279 | Serial.println("error opening forcedata.txt"); 280 | } 281 | } 282 | 283 | void checkTorque() 284 | { 285 | Torque = (scale.get_units(1)*9.81)*0.1945; 286 | Serial.print("Motor Torque: "); 287 | Serial.print(Torque); 288 | Serial.print("\t"); 289 | Serial.print((timer*timeout)/100); 290 | Serial.print("\t"); 291 | 292 | if (RPMdataFile) 293 | { 294 | RPMdataFile.print((timer*timeout)/1000); 295 | RPMdataFile.print(","); 296 | RPMdataFile.print(rpm); 297 | RPMdataFile.print(","); 298 | RPMdataFile.println(Torque); 299 | } 300 | else 301 | { 302 | // if the file didn't open, print an error: 303 | Serial.println("error opening forcedata.txt"); 304 | } 305 | } 306 | 307 | void setScales() 308 | { 309 | Serial.println("Before setting up the scale:"); 310 | Serial.print("read: \t\t"); 311 | Serial.println(scale.read()); // print a raw reading from the ADC 312 | 313 | Serial.print("read average: \t\t"); 314 | Serial.println(scale.read_average(20)); // print the average of 20 readings from the ADC 315 | 316 | Serial.print("get value: \t\t"); 317 | Serial.println(scale.get_value(5)); // print the average of 5 readings from the ADC minus the tare weight (not set yet) 318 | 319 | Serial.print("get units: \t\t"); 320 | Serial.println(scale.get_units(5), 1); // print the average of 5 readings from the ADC minus tare weight (not set) divided 321 | // by the SCALE parameter (not set yet) 322 | 323 | scale.set_scale(777.1f); // this value is obtained by calibrating the scale with known weights; see the README for details 324 | scale.tare(); // reset the scale to 0 325 | 326 | Serial.println("After setting up the scale:"); 327 | 328 | Serial.print("read: \t\t"); 329 | Serial.println(scale.read()); // print a raw reading from the ADC 330 | 331 | Serial.print("read average: \t\t"); 332 | Serial.println(scale.read_average(20)); // print the average of 20 readings from the ADC 333 | 334 | Serial.print("get value: \t\t"); 335 | Serial.println(scale.get_value(5)); // print the average of 5 readings from the ADC minus the tare weight, set with tare() 336 | 337 | Serial.print("get units: \t\t"); 338 | Serial.println(scale.get_units(5), 1); // print the average of 5 readings from the ADC minus tare weight, divided 339 | // by the SCALE parameter set with set_scale 340 | } 341 | -------------------------------------------------------------------------------- /Modelling/Simulation.m: -------------------------------------------------------------------------------- 1 | close all 2 | clc 3 | 4 | Time = 100; 5 | kT = round(Time/T); 6 | 7 | X = zeros(14,kT); 8 | Xreal = zeros(18,kT); 9 | 10 | U = U_e*ones(6,kT); 11 | 12 | Y = zeros(4,kT); 13 | Xe = zeros(4,kT); 14 | 15 | Ref = [0;0;0;0]; 16 | x_ini = [0;0;0;0;0;0;0;0;0;0;0;0;0;0]; 17 | 18 | X(:,2) = x_ini; 19 | Xest = X; 20 | Xest(:,1) = x_ini+0.001*randn(14,1); 21 | Xreal(5:end,2) = x_ini; 22 | U(:,1) = 0; 23 | 24 | for k = 2:kT-1 25 | 26 | %Estimation 27 | %Xest(:,k) = Adt*Xest(:,k-1)+Bdt*(U(:,k-1)-U_e); %Linear Prediction Phase 28 | t_span = [0,T]; 29 | xkf = [0;0;0;0;Xest(:,k-1)]; %Remapping 30 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k-1)),t_span,xkf); %Nonlinear Prediction 31 | Xest(:,k) = xode.y(5:18,end); %Remappping back 32 | Y(:,k) = Xreal([5,7,9,11],k); 33 | Pred_Error = [Y(:,k) - Xest([1,3,5,7],k); 0; 0]; 34 | Xest(:,k) = Xest(:,k) + Ldt*Pred_Error; 35 | 36 | %Control 37 | %Xest(:,k) = Xreal([5,6,7,10,8,11,9,12,13:18],k); %No Kalman Filter 38 | Xe(:,k) = Xe(:,k-1) + (Ref - Xest([1,3,5,7],k)); 39 | U(:,k) = U_e - [Kdt,Kidt]*[Xest(:,k) - [Ref(1);0;Ref(2);0;Ref(3);0;Ref(4);0;W_e*ones(6,1)]; Xe(:,k)]; 40 | 41 | %Simulation 42 | t_span = [0,T]; 43 | xode = ode45(@(t,X) Hex_Dynamics(t,X,U(:,k)),t_span,Xreal(:,k)); 44 | Xreal(:,k+1) = xode.y(:,end); 45 | 46 | %%%%% Forward Euler Nonlinear Dynamics %%%%%%% 47 | % [dX]=Hex_Dynamics(t,Xreal(:,k),U(:,k)); 48 | % Xreal(:,k+1)=Xreal(:,k)+T*dX; 49 | 50 | %%%%% Fully Linear Dynamics %%%%%% 51 | % X(:,k+1)=Adt*X(:,k)+Bdt*U(:,k); 52 | 53 | % if k == 10/T 54 | % Ref(1) = 1; 55 | % end 56 | % if k == 20/T 57 | % Ref(2) = 30*pi/180; 58 | % end 59 | % if k == 30/T 60 | % Ref(3) = 30*pi/180; 61 | % end 62 | if k == 40/T 63 | Ref(4) = 30*pi/180; 64 | end 65 | end 66 | 67 | %Plots 68 | t = (0:kT-1)*T; 69 | figure(1); 70 | subplot(3,1,1); 71 | plot(t,Xreal([5,7,9,11],:)) 72 | legend('Alt','\phi','\theta','\psi') 73 | subplot(3,1,2); 74 | plot(t,Xest([1,3,5,7],:)) 75 | legend('Alt_e','\phi_e','\theta_e','\psi_e') 76 | subplot(3,1,3); 77 | plot(t,U); 78 | -------------------------------------------------------------------------------- /Modelling/Tracking_Example.m: -------------------------------------------------------------------------------- 1 | %Model 2 | sys = ss(A,B,C,0); 3 | sysd = c2d(sys,T); 4 | Ad = sysd.A; 5 | Bd = sysd.B; 6 | Cd = sysd.C; 7 | 8 | %LQR Design 9 | Q = diag([0.01,2000,100,0,100,0,1,0,0,0,0,0,0,0]); % State penalty 10 | R = (1*10^-3)*eye(6,6); % Control penalty 11 | 12 | Kx = dlqr(Ad,Bd,Q,R,0); 13 | 14 | %Simulation Parameters 15 | Time = 20; 16 | dt = T; 17 | kT = round(Time/dt); 18 | X = zeros(14,kT); 19 | Y = zeros(4,kT); 20 | U = zeros(6,kT); 21 | X(:,1) = [0;0;0;0;0;0;0;0;0;0;0;0;0;0]; 22 | 23 | for k = 1:kT-1 24 | %Control System 25 | Ref = [0;0;0;20*pi/180]; 26 | U(:,k )= -Kx*(X(:,k)-[Ref(1);0;Ref(2);0;Ref(3);0;Ref(4);0;0;0;0;0;0;0]); 27 | 28 | %Simulate 29 | X(:,k+1) = Ad*X(:,k)+Bd*U(:,k); 30 | Y(:,k) = Cd*X(:,k); 31 | end 32 | 33 | %Plot 34 | t = dt*[0:kT-1]; 35 | 36 | figure(3); 37 | subplot(4,1,1); 38 | plot(t,Y(1,:)); 39 | subplot(4,1,2); 40 | plot(t,Y(2,:)*(180/pi)); 41 | subplot(4,1,3); 42 | plot(t,Y(3,:)*(180/pi)); 43 | subplot(4,1,4); 44 | plot(t,Y(4,:)*(180/pi)); 45 | 46 | figure(4); 47 | plot(t,U'); 48 | -------------------------------------------------------------------------------- /Modelling/Y6MultirotorOpenLoopModelSimulation.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Modelling/Y6MultirotorOpenLoopModelSimulation.slx -------------------------------------------------------------------------------- /Modelling/Y6MultirotorOpenLoopModelSimulation2.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/Modelling/Y6MultirotorOpenLoopModelSimulation2.slx -------------------------------------------------------------------------------- /Modelling/plotUAV.m: -------------------------------------------------------------------------------- 1 | theta = linspace(0,2*pi); 2 | % xc = cos(theta); 3 | % yc = -sin(theta); 4 | % plot(xc,yc); 5 | figure(1) 6 | axis equal 7 | xt = [-1 -1 1 1]; 8 | yt = [0 -0.2 -0.2 0]; 9 | hold on 10 | t = area(xt,yt); % initial flat triangle 11 | hold on 12 | p = plot(0,0,'o','MarkerFaceColor','black'); 13 | hold off 14 | for j = 1:length(theta) 15 | xt(1) = -1*cos(theta(j)); 16 | xt(2) = -1*cos(theta(j))-(0.2*sin(theta(j))); 17 | xt(3) = 1*cos(theta(j))-(0.2*sin(theta(j))); % determine new vertex value 18 | xt(4) = 1*cos(theta(j)); 19 | 20 | yt(1) = 1*sin(theta(j)); 21 | yt(2) = 1*sin(theta(j))-(0.2*cos(theta(j))); 22 | yt(3) = -1*sin(theta(j))-(0.2*cos(theta(j))) ; 23 | yt(4) = -1*sin(theta(j)); 24 | t.XData = xt; % update data properties 25 | t.YData = yt; 26 | t.BASEVALUE = yt(4); 27 | drawnow limitrate % display updates 28 | pause(0.1); 29 | end -------------------------------------------------------------------------------- /PID-8States/ControlDesignPID.m: -------------------------------------------------------------------------------- 1 | close all; % close all figures 2 | clear; % clear workspace variables 3 | clc; % clear command window 4 | format short; 5 | 6 | %% Mass of the Multirotor in Kilograms as taken from the CAD 7 | 8 | M = 1.455882; 9 | g = 9.81; 10 | 11 | %% Dimensions of Multirotor 12 | 13 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 14 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 15 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 16 | 17 | %% Mass Moment of Inertia as Taken from the CAD 18 | 19 | Ixx = 0.014; 20 | Iyy = 0.028; 21 | Izz = 0.038; 22 | 23 | %% Motor Thrust and Torque Constants (To be determined experimentally) 24 | 25 | Kw = 0.85; 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 | 34 | Dxx = 0.01212; 35 | Dyy = 0.01212; 36 | Dzz = 0.0648; 37 | 38 | %% Equilibrium Input 39 | 40 | %W_e = sqrt(((M*g)/(3*(Kthrust+(Kw*Kthrust))))); 41 | W_e = ((-6*Kthrust2) + sqrt((6*Kthrust2)^2 - (4*(-M*g)*(3*Kw*Kthrust + 3*Kthrust))))/(2*(3*Kw*Kthrust + 3*Kthrust)); 42 | U_e = (W_e/Ku); 43 | 44 | %% Define Discrete-Time BeagleBone Dynamics 45 | 46 | T = 0.01; % Sample period (s)- 100Hz 47 | ADC = 3.3/((2^12)-1); % 12-bit ADC Quantization 48 | DAC = 3.3/((2^12)-1); % 12-bit DAC Quantization 49 | 50 | %% Define Linear Continuous-Time Multirotor Dynamics: x_dot = Ax + Bu, y = Cx + Du 51 | 52 | % A = 8x8 matrix 53 | A = [0, 1, 0, 0, 0, 0, 0, 0; 54 | 0, 0, 0, 0, 0, 0, 0, 0; 55 | 0, 0, 0, 1, 0, 0, 0, 0; 56 | 0, 0, 0, 0, 0, 0, 0, 0; 57 | 0, 0, 0, 0, 0, 1, 0, 0; 58 | 0, 0, 0, 0, 0, 0, 0, 0; 59 | 0, 0, 0, 0, 0, 0, 0, 1; 60 | 0, 0, 0, 0, 0, 0, 0, 0]; 61 | 62 | % B = 8x6 matrix 63 | B = [0, 0, 0, 0, 0, 0; 64 | 2*Kthrust*W_e*((Ku))/M, 2*Kw*Kthrust*W_e*((Ku))/M, 2*Kthrust*W_e*((Ku))/M, 2*Kw*Kthrust*W_e*((Ku))/M, 2*Kthrust*W_e*((Ku))/M, 2*Kw*Kthrust*W_e*((Ku))/M; 65 | 0, 0, 0, 0, 0, 0; 66 | 2*L1*Kthrust*W_e*((Ku))/Ixx, 2*L1*Kw*Kthrust*W_e*((Ku))/Ixx, -2*L1*Kthrust*W_e*((Ku))/Ixx, -2*L1*Kw*Kthrust*W_e*((Ku))/Ixx, 0, 0; 67 | 0, 0, 0, 0, 0, 0; 68 | -2*L2*Kthrust*W_e*((Ku))/Iyy, -2*L2*Kw*Kthrust*W_e*((Ku))/Iyy, -2*L2*Kthrust*W_e*((Ku))/Iyy, -2*L2*Kw*Kthrust*W_e*((Ku))/Iyy, 2*L3*Kthrust*W_e*((Ku))/Iyy,2*L3*Kw*Kthrust*W_e*((Ku))/Iyy; 69 | 0, 0, 0, 0, 0, 0; 70 | -2*Ktau/Izz*W_e*((Ku)), 2*Ktau*W_e*((Ku))/Izz, 2*Ktau*W_e*((Ku))/Izz, -2*Ktau*W_e*((Ku))/Izz, -2*Ktau*W_e*((Ku))/Izz, 2*Ktau*W_e*((Ku))/Izz]; 71 | 72 | % C = 4x8 matrix 73 | C = [1, 0, 0, 0, 0, 0, 0, 0; 74 | 0, 0, 1, 0, 0, 0, 0, 0; 75 | 0, 0, 0, 0, 1, 0, 0, 0; 76 | 0, 0, 0, 0, 0, 0, 1, 0]; 77 | 78 | % D = 4x6 matrix 79 | D = 0; 80 | 81 | %% System Characteristics 82 | 83 | poles = eig(A); 84 | Jpoles = jordan(A); 85 | % System Unstable 86 | 87 | cntr = rank(ctrb(A,B)); 88 | % Fully Reachable 89 | 90 | obs = rank(obsv(A,C)); 91 | % Fully Observable 92 | 93 | %% Output and Motor Mix Matrix 94 | 95 | K = [1, 1, -1, 1; 96 | 1, 1, -1, -1; 97 | 1, -1, -1, -1; % Motor Mixer 98 | 1, -1, -1, 1; 99 | 1, 0, 1, 1; 100 | 1, 0, 1, -1]; 101 | 102 | Bp = B*K; 103 | Dp = zeros(4,4); 104 | 105 | %% SISO System Outputs 106 | 107 | s = tf('s'); 108 | I = eye(8); 109 | 110 | G = (C * inv(s*I - A) * Bp) + Dp; 111 | 112 | Gz = c2d(G(1,1),T); 113 | Gphi = c2d(G(2,2),T); 114 | Gtheta = c2d(G(3,3),T); 115 | Gpsi = c2d(G(4,4),T); 116 | 117 | %% Discrete-Time Kalman Filter Design x_dot = A*x + B*u + G*w, y = C*x + D*u + H*w + v 118 | 119 | sysdt = c2d(ss(A,B,C,D),T,'zoh'); % Generate Discrete-Time System 120 | 121 | Adt = sysdt.a; 122 | Bdt = sysdt.b; 123 | Cdt = sysdt.c; 124 | Ddt = sysdt.d; 125 | 126 | n = size(Adt,2); 127 | Gdt = 1e-1*eye(n); 128 | Hdt = zeros(size(C,1),size(Gdt,2)); % No process noise on measurements 129 | 130 | Rw = diag([0.000000001,1,0.000000001,1,0.000000001,1,0.000000001,1]); % Process noise covariance matrix 131 | Rv = diag([1,1,1,1])*10^-5; % Measurement noise covariance matrix Note: use low gausian noice for Rv 132 | N = zeros(size(Rw,2),size(Rv,2)); 133 | 134 | sys4kf = ss(Adt,[Bdt Gdt],Cdt,[Ddt Hdt],T); 135 | 136 | [kdfilt,Ldt] = kalman(sys4kf,Rw,Rv); 137 | -------------------------------------------------------------------------------- /PID-8States/Hex_Dynamics.m: -------------------------------------------------------------------------------- 1 | function [dX]=Hex_Dynamics(t,X,U) 2 | %% Mass of the Multirotor in Kilograms as taken from the CAD 3 | 4 | M = 1.455882; 5 | g = 9.81; 6 | 7 | %% Dimensions of Multirotor 8 | 9 | L1 = 0.19; % along X-axis Distance from left and right motor pair to center of mass 10 | L2 = 0.18; % along Y-axis Vertical Distance from left and right motor pair to center of mass 11 | L3 = 0.30; % along Y-axis Distance from motor pair to center of mass 12 | 13 | %% Mass Moment of Inertia as Taken from the CAD 14 | % Inertia Matrix and Diagolalisation CAD model coordinate system rotated 90 degrees about X 15 | 16 | Ixx = 0.014; 17 | Iyy = 0.028; 18 | Izz = 0.038; 19 | 20 | %% Motor Thrust and Torque Constants (To be determined experimentally) 21 | 22 | Kw = 0.85; 23 | Ktau = 7.708e-10; 24 | Kthrust = 1.812e-07; 25 | Kthrust2 = 0.0007326; 26 | Mtau = (1/44.22); 27 | Ku = 515.5*Mtau; 28 | 29 | %% Air resistance damping coeeficients 30 | 31 | Dxx = 0.01212; 32 | Dyy = 0.01212; 33 | Dzz = 0.0648; 34 | 35 | %% X = [x xdot y ydot z zdot phi p theta q psi r w1 w2 w3 w4 w5 w6] 36 | 37 | x = X(1); 38 | xdot = X(2); 39 | y = X(3); 40 | ydot = X(4); 41 | z = X(5); 42 | zdot = X(6); 43 | 44 | phi = X(7); 45 | p = X(8); 46 | theta = X(9); 47 | q = X(10); 48 | psi = X(11); 49 | r = X(12); 50 | 51 | w1 = X(13); 52 | w2 = X(14); 53 | w3 = X(15); 54 | w4 = X(16); 55 | w5 = X(17); 56 | w6 = X(18); 57 | 58 | %% Initialise Outputs 59 | 60 | dX = zeros(18,1); 61 | Y = zeros(4,1); 62 | 63 | %% Motor Dynamics: dX = [w1dot w2dot w3dot w4dot w5dot w6dot], U = Pulse Width of the pwm signal 0-1000% 64 | 65 | dX(13) = -(1/Mtau)*w1 + Ku/Mtau*U(1); 66 | dX(14) = -(1/Mtau)*w2 + Ku/Mtau*U(2); 67 | dX(15) = -(1/Mtau)*w3 + Ku/Mtau*U(3); 68 | dX(16) = -(1/Mtau)*w4 + Ku/Mtau*U(4); 69 | dX(17) = -(1/Mtau)*w5 + Ku/Mtau*U(5); 70 | dX(18) = -(1/Mtau)*w6 + Ku/Mtau*U(6); 71 | 72 | %% Motor Forces and Torques 73 | 74 | F = zeros(6,1); 75 | T = zeros(6,1); 76 | 77 | F(1)= Kw*Kthrust*(w1^2) + Kthrust2*w1; 78 | F(2)= Kthrust*(w2^2) + Kthrust2*w2; 79 | F(3)= Kw*Kthrust*(w3^2)+ Kthrust2*w3; 80 | F(4)= Kthrust*(w4^2) + Kthrust2*w4; 81 | F(5)= Kw*Kthrust*(w5^2) + Kthrust2*w5; 82 | F(6)= Kthrust*(w6^2) + Kthrust2*w6; 83 | 84 | T(1)= -Ktau*(w1^2); 85 | T(2)= Ktau*(w2^2); 86 | T(3)= Ktau*(w3^2); 87 | T(4)= -Ktau*(w4^2); 88 | T(5)= -Ktau*(w5^2); 89 | T(6)= Ktau*(w6^2); 90 | 91 | Fn = sum(F); 92 | Tn = sum(T); 93 | 94 | %% First Order Direvatives dX = [xdot ydot zdot phidot thetadot psidot] 95 | 96 | dX(1) = X(2); 97 | dX(3) = X(4); 98 | dX(5) = X(6); 99 | dX(7) = p + q*(sin(phi)*tan(theta)) + r*(cos(phi)*tan(theta)); 100 | dX(9) = q*cos(phi) - r*sin(phi); 101 | dX(11) = q*(sin(phi)/cos(theta)) + r*(cos(phi)/cos(theta)); 102 | 103 | %% Second Order Direvatives: dX = [xddot yddot zddot pdot qdot rdot] 104 | 105 | dX(2) = Fn/M*(cos(phi)*sin(theta)*cos(psi)) + Fn/M*(sin(phi)*sin(psi)) - (Dxx/M)*xdot; 106 | dX(4) = Fn/M*(cos(phi)*sin(theta)*sin(psi)) - Fn/M*(sin(phi)*cos(psi)) - (Dyy/M)*ydot; 107 | dX(6) = Fn/M*(cos(phi)*cos(theta)) - g - (Dzz/M)*zdot; 108 | 109 | dX(8) = (L1/Ixx)*((F(1)+F(2)) - (F(3)+F(4))) - ((Izz-Iyy)/Ixx)*(r*q); 110 | dX(10) = (L3/Iyy)*(F(5)+F(6)) - (L2/Iyy)*(F(1)+F(2)+F(3)+F(4)) - ((Izz-Ixx)/Iyy)*(p*r); 111 | dX(12) = Tn/Izz - ((Iyy-Ixx)/Izz)*(p*q); 112 | 113 | %% Measured States 114 | 115 | Y(1) = z; 116 | Y(2) = phi; 117 | Y(3) = theta; 118 | Y(4) = psi; 119 | 120 | end -------------------------------------------------------------------------------- /PID-8States/Y6MultirotorPIDSimulation.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alexdada555/Modelling-Simulation-and-Implementation-of-Linear-Control-for-Asymmetric-Multirotor-UAVs/238705b03e07eaff53bce649c0005b1436916b6a/PID-8States/Y6MultirotorPIDSimulation.slx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Modelling, Simulation, and Implementation of Linear Control for Asymmetric Multirotor Unmanned Aerial Vehicles 2 | The basic framework of this investigative project is set around the design and development of control software for Vertical/Short Take-off and Landing (VSTOL) model aircraft. From this theme, the aims of modelling and simulating the non-linear dynamics of an asymmetrical VTOL multi-rotor platform and the development of multiple control schemes, such as Proportional Integral Derivative, Linear Quadratic Gaussian and Model Predictive control algorithms, were derived. These control schemes will then be implemented on the modelled multi-rotor platform via an embedded microprocessor unit. 3 | 4 | These aims break down into multiple objective and milestones which must each be achieved to fulfil the full scope of that the aims outlined. These objectives can be broadly classified into 2 categories: 5 | 6 | Basic Objectives 7 | 1. Develop a mathematical model representing the dynamics of the multi-rotor aircraft. 8 | 2. Develop a dynamic simulation of the crafts behaviour. 9 | 3. Develop feedback control laws: Linear Quadratic Gaussian (LQG). 10 | 4. Investigate feedback control laws in simulation with the mathematical model to achieve behavioural targets. 11 | 5. Develop flight control software to interface with sensors and implement control laws. 12 | 6. Implement the flight control software on an embedded microprocessor unit. 13 | 7. Implement controller unto multi-rotor platform. 14 | 8. Discuss the results of performance comparisons between the simulation and hardware implementations. 15 | 16 | Advanced Objectives 17 | 1. Investigate the application of Model Predictive Control (MPC) schemes in simulation. 18 | 2. Incorporate and implement MPC schemes unto the flight control software. 19 | 3. Discuss the differences between the LQG and MPC implementations 20 | 21 | --------------------------------------------------------------------------------