├── README.md ├── attitude-control ├── Solver_attitude.asv ├── Solver_attitude.m └── test │ ├── test_dim_times.m │ ├── test_min_ndim.m │ ├── test_simplified.m │ └── test_simplified_20deg.m ├── pos-att ├── Solver_pos_att.m └── private │ ├── fDot_and_gDot.m │ ├── f_and_g.m │ ├── kepler_U.m │ ├── rkf45.m │ ├── stumpC.m │ ├── stumpS.m │ └── sv_from_coe.m ├── position-control ├── Solver_position.m └── private │ ├── fDot_and_gDot.m │ ├── f_and_g.m │ ├── kepler_U.m │ ├── rkf45.m │ ├── stumpC.m │ ├── stumpS.m │ └── sv_from_coe.m └── test ├── Dynamic_Solver.asv ├── Dynamic_Solver.m ├── obj_1.mat ├── obj_1.txt ├── result-obj_1.png ├── target-obj_1.png ├── test_coder.m ├── test_griddedInterp.m ├── test_performance_Interpolant.m ├── test_performance_X_inline.m ├── test_performance_find.m ├── test_performance_ii.m └── test_u_star_M.m /README.md: -------------------------------------------------------------------------------- 1 | 2 | # Legacy Repository 3 | 4 | This code repository contains codes for Dynamic Programming with application in Optimal Control for seperate position control and attitude control. It is now re-designed to incorporate full coupled position and attitude control of a satellite, see the successor repository: https://github.com/abdolrezat/SPHERES-DPcontrol 5 | 6 | 7 | ## Run Test Class 8 | 9 | MATLAB Optimal Control codes related to HJB Dynamic Programming to find the optimal path for any state of a linear system 10 | 11 | The Test Class solves the example at the end of chapter 3 of Optimal Control Theory - kirk (System with state equation A*X + B*U 12 | ) 13 | 14 | ### Run the Tests 15 | The class is loaded with a example. In order to see the results, create a Dynamic_Solver object first: 16 | `objA = Dynamic_Solver;` 17 | Calculate the optimal controls at each state: 18 | `run(objA)` 19 | Finally, track the optimal states and controls: 20 | `get_optimal_path(objA)` 21 | or get the optimal states and controls from any desired initial states: 22 | `get_optimal_path(objA, [x1 x2])` 23 | 24 | ### Unique Performance 25 | This code exploits Vectorization in MATLAB thus it is more than thousands orders of magnitude faster than loop algorithm implementation which is explained in Optimal Control Theory book(s). 26 | -------------------------------------------------------------------------------- /attitude-control/Solver_attitude.asv: -------------------------------------------------------------------------------- 1 | classdef Solver_attitude < dynamicprops 2 | %SOLVER_ATTITUDE Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | N % number of stages 7 | J1 8 | J2 9 | J3 10 | size_state_mat 11 | dim_U1 % dimension number of U, after the 7 state variables 12 | dim_U2 13 | dim_U3 14 | U_vector % values that are applied as control inputs 15 | % big data 16 | J_next_states_opt single % optimum values of J will be stored in this matrix and recalled each stage 17 | J_current_state_fix single % values of cost to reach next stage, always fixed in Time Invariant system 18 | %obsolete, use X1V,etc... instead 19 | % X1 single % (s+c)-dimensional grid of state 1 20 | % X2 single 21 | % X3 single 22 | % X4 single 23 | % X5 single 24 | % X6 single 25 | % X7 single 26 | % U1 single % (s+c)-dimensional grid of control input 1 27 | % U2 single 28 | % U3 single 29 | X1_next single % (s+c)-dim grid of next state-1 30 | X2_next single 31 | X3_next single 32 | % X4_next single 33 | X5_next single 34 | X6_next single 35 | X7_next single 36 | 37 | U1_Opt % (s-dim x k) grid of optimal values U* for every stage and states 38 | U2_Opt 39 | U3_Opt 40 | %-- ranges for grid generation 41 | w_min 42 | w_max 43 | n_mesh_w 44 | yaw_min 45 | yaw_max 46 | pitch_min 47 | pitch_max 48 | roll_min 49 | roll_max 50 | n_mesh_q 51 | sr_1 % state 1 range vector for grid and interpolant generation 52 | sr_2 % " 53 | sr_3 % " 54 | % sr_4 % " 55 | s_yaw % state 5 56 | s_pitch % " 6 57 | s_roll % " 7 58 | cr_1 % control input 1 range vector for grid generation 59 | cr_2 60 | cr_3 61 | 62 | T_final 63 | h %time step for discrete time system 64 | N_stage 65 | 66 | Q1 67 | Q2 68 | Q3 69 | Q5 70 | Q6 71 | Q7 72 | R1 73 | R2 74 | R3 75 | 76 | X1V single 77 | X2V single 78 | X3V single 79 | cang_x5 single 80 | sang_x5 single 81 | cang_x6 single 82 | sang_x6 single 83 | cang_x7 single 84 | sang_x7 single 85 | U1V single 86 | U2V single 87 | U3V single 88 | end 89 | 90 | methods 91 | %Constructor 92 | function this = Solver_attitude() 93 | % states are [w1; w2; w3; q1; q2; q3; q4] 94 | if nargin < 1 95 | w_min = -2; 96 | w_max = 2; 97 | n_mesh_w = 11; 98 | this.yaw_min = -10; %angles 99 | this.yaw_max = 10; 100 | this.pitch_min = -10; 101 | this.pitch_max = 10; 102 | this.roll_min = -10; 103 | this.roll_max = 10; 104 | this.n_mesh_q = 10; 105 | 106 | this.Q1 = 5; 107 | this.Q2 = 5; 108 | this.Q3 = 5; 109 | this.Q5 = 5; 110 | this.Q6 = 5; 111 | this.Q7 = 5; 112 | this.R1 = 0.5; 113 | this.R2 = 0.5; 114 | this.R3 = 0.5; 115 | 116 | this.T_final = 1; 117 | this.h = 0.05; 118 | end 119 | this.w_min = w_min; 120 | this.w_max = w_max; 121 | this.n_mesh_w = n_mesh_w; 122 | this.N_stage = this.T_final/this.h; 123 | 124 | this.J1 = 2; 125 | this.J2 = 2.5; 126 | this.J3 = 3; 127 | this.dim_U1 = 7; 128 | this.dim_U2 = 8; 129 | this.dim_U3 = 9; 130 | this.U_vector = [-0.01 0 0.01]; 131 | 132 | %Preallocation and mesh generation 133 | this.sr_1 = linspace(w_min, w_max, n_mesh_w); 134 | this.sr_2 = linspace(w_min, w_max, n_mesh_w); 135 | this.sr_3 = linspace(w_min, w_max, n_mesh_w); 136 | 137 | this.s_yaw = linspace(deg2rad(this.yaw_min), deg2rad(this.yaw_max), this.n_mesh_q); 138 | this.s_pitch = linspace(deg2rad(this.pitch_min), deg2rad(this.pitch_max), this.n_mesh_q); 139 | this.s_roll = linspace(deg2rad(this.roll_min), deg2rad(this.roll_max), this.n_mesh_q); 140 | 141 | this.size_state_mat = [n_mesh_w,n_mesh_w,n_mesh_w,... 142 | this.n_mesh_q,this.n_mesh_q,this.n_mesh_q]; 143 | size_Umat = [this.size_state_mat,3,this.N_stage]; 144 | this.U1_Opt = zeros(size_Umat,'uint8'); 145 | this.U2_Opt = zeros(size_Umat,'uint8'); 146 | this.U3_Opt = zeros(size_Umat,'uint8'); 147 | 148 | end 149 | 150 | %Calculation of optimal matrices 151 | function obj = run(obj) 152 | obj.reshape_states(); 153 | % calculate cost to reach next stage 154 | %obj.J_current_state_fix = g_D(obj); 155 | fprintf('calculating fixed cost matrix...\n') 156 | calculate_J_current_state_fix_shaped(obj); 157 | % Calculate and store J*NN = h(xi(N)) for all x(N) 158 | obj.J_next_states_opt = zeros(obj.size_state_mat,'single'); 159 | % 160 | fprintf('calculating next stage states...\n') 161 | calculate_states_next(obj); 162 | 163 | % 164 | %% obj.stage_J_star = H*X; 165 | %% 166 | fprintf('beginning stage calculation...\n') 167 | for k_s=obj.N_stage-1:-1:1 168 | tic 169 | calculate_J_U_opt_state_M(obj, k_s); 170 | fprintf('step %d - %f seconds\n', k_s, toc) 171 | end 172 | fprintf('stage calculation complete.\n') 173 | 174 | end 175 | 176 | function [X1,X2,X3,X4,X5,X6,X7] = a_D_M(obj, X1,X2,X3,X4,X5,X6,X7,U1,U2,U3) 177 | 178 | 179 | end 180 | 181 | function calculate_J_current_state_fix(obj) 182 | obj.J_current_state_fix = obj.Q1*obj.X1.^2 + obj.Q2*obj.X2.^2 + ... 183 | obj.Q3*obj.X3.^2 + ... % obj.Q4*obj.X4.^2 + ... 184 | obj.Q5*obj.X5.^2 + obj.Q6*obj.X6.^2 + ... 185 | obj.Q7*obj.X7.^2 + obj.R1*obj.U1.^2 + ... 186 | obj.R2*obj.U2.^2 + obj.R3*obj.U3.^2; 187 | end 188 | 189 | function calculate_J_current_state_fix_shaped(obj) 190 | obj.J_current_state_fix = obj.Q1*obj.X1V.^2 + obj.Q2*obj.X2V.^2 + ... 191 | obj.Q3*obj.X3V.^2 + ... % obj.Q4*obj.X4.^2 + ... 192 | obj.Q5*(obj.cang_x5.*obj.cang_x6.*obj.sang_x7 - obj.sang_x5.*obj.sang_x6.*obj.cang_x7).^2 + ... 193 | obj.Q6*(obj.cang_x5.*obj.sang_x6.*obj.cang_x7 + obj.sang_x5.*obj.cang_x6.*obj.sang_x7).^2 + ... 194 | obj.Q7*(obj.sang_x5.*obj.cang_x6.*obj.cang_x7 - obj.cang_x5.*obj.sang_x6.*obj.sang_x7).^2 + ... 195 | obj.R1*obj.U1V.^2 + obj.R2*obj.U2V.^2 + obj.R3*obj.U3V.^2; 196 | %% 197 | %q's are defined as follows: 198 | %x4 = q4 as defined in Kirk Control Theory, q1 by MATLAB 199 | %definition 200 | %(obj.cang_x5.*obj.cang_x6.*obj.cang_x7 + obj.sang_x5.*obj.sang_x6.*obj.sang_x7) 201 | 202 | %x5 = q3 as defined in Kirk Control Theory, q2 by MATLAB 203 | %definition 204 | %(obj.cang_x5.*obj.cang_x6.*obj.sang_x7 - obj.sang_x5.*obj.sang_x6.*obj.cang_x7) 205 | 206 | %x6 = q2 as defined in Kirk Control Theory, q3 by MATLAB 207 | %definition 208 | %%(obj.cang_x5.*obj.sang_x6.*obj.cang_x7 + obj.sang_x5.*obj.cang_x6.*obj.sang_x7) 209 | 210 | %x7 = q1 as defined in Kirk Control Theory, q4 by MATLAB 211 | %definition 212 | %(obj.sang_x5.*obj.cang_x6.*obj.cang_x7 - obj.cang_x5.*obj.sang_x6.*obj.sang_x7) 213 | 214 | %% 215 | 216 | end 217 | 218 | function calculate_states_next(obj) 219 | %calculates next stage (k+1) states 220 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 221 | 222 | % Runge-Kutta - 4th order 223 | % h = dt; 224 | % [obj.X1_next, obj.X2_next, obj.X3_next, obj.X4_next,... 225 | % obj.X5_next, obj.X6_next, obj.X7_next] = ... 226 | % spacecraft_dynamics(spacecraft, obj.X1, obj.X2, obj.X3, obj.X4, obj.X5, obj.X6,... 227 | % obj.X7, obj.U1, obj.U2, obj.U3); %X2 = k1 228 | % 229 | % [obj.X1_next, obj.X2_next, obj.X3_next, obj.X4_next,... 230 | % obj.X5_next, obj.X6_next, obj.X7_next] = ... 231 | % spacecraft_dynamics(spacecraft, obj.X1 + obj.X1_next*h/2 , obj.X2 + obj.X2_next*h/2,... 232 | % obj.X3 + obj.X3_next*h/2 , obj.X4 + obj.X4_next*h/2 , obj.X5 + obj.X5_next*h/2 ,... 233 | % obj.X6 + obj.X6_next*h/2 ,... 234 | % obj.X7 + obj.X7_next*h/2 , obj.U1, obj.U2, obj.U3); % k = k2 235 | % 236 | % k = spacecraft_dynamics(spacecraft, (X1 + X2*h/2), U); 237 | % X2 = X2 + 2*k; % X2 = k1 + 2*k2 238 | % k = spacecraft_dynamics(spacecraft, (X1 + k*h/2), U); % k = k3 239 | % X2 = X2 + 2*k; % X2 = k1 + 2*k2 + 2*k3 240 | % k = spacecraft_dynamics(spacecraft, (X1 + k*h), U); % k = k4 241 | % X2 = X2 + k; 242 | % X2 = X1 + h/6*(X2); 243 | 244 | 245 | %% taylor expansion 246 | spacecraft_dynamics_taylor_estimate(obj); 247 | % 248 | end 249 | 250 | function J = g_D(obj,X1,X2,X3,X5,X6,X7,U1,U2,U3) 251 | J = obj.Q1*X1.^2 + obj.Q2*X2.^2 + ... 252 | obj.Q3*X3.^2 + ... %obj.Q4*X4.^2 + ... 253 | obj.Q5*X5.^2 + obj.Q6*X6.^2 + ... 254 | obj.Q7*X7.^2 + obj.R1*U1.^2 + ... 255 | obj.R2*U2.^2 + obj.R3*U3.^2; 256 | end 257 | 258 | function calculate_J_U_opt_state_M(obj, k_s) 259 | %% CAUTION: this interpolant is only valid for Xmesh 260 | keyboard 261 | F = griddedInterpolant(... 262 | {obj.sr_1, obj.sr_2, obj.sr_3, obj.s_yaw, obj.s_pitch, obj.s_roll}, ... 263 | obj.J_next_states_opt,'linear'); 264 | 265 | %find J final for each state and control (X,U) and add it to next state 266 | %optimum J* 267 | [val, U_ID3] = min( obj.J_current_state_fix + ... 268 | F(obj.X1_next,... %X1_next size = n_mesh_w 269 | obj.X2_next,... 270 | obj.X3_next,... 271 | obj.X5_next,... 272 | obj.X6_next,... 273 | obj.X7_next), ... 274 | [], obj.dim_U3); 275 | [val, U_ID2] = min( val, [], obj.dim_U2); 276 | [obj.J_next_stages_opt , U_ID1] = min( val, [], obj.dim_U1); 277 | 278 | obj.U1_Opt(:,:,:,:,:,:,k_s) = obj.U_vector(U_ID1); 279 | obj.U2_Opt(:,:,:,:,:,:,k_s) = obj.U_vector(U_ID2(U_ID1)); 280 | obj.U3_Opt(:,:,:,:,:,:,k_s) = obj.U_vector(U_ID3(U_ID2(U_ID1))); 281 | 282 | end 283 | 284 | function spacecraft_dynamics_taylor_estimate(obj) 285 | %returns the derivatives x_dot = f(X,u) 286 | %J is assumed to be diagonal, J12 = J23 = ... = 0 287 | x4 = 1 - ((obj.sang_x5.*obj.cang_x6.*obj.cang_x7 - obj.cang_x5.*obj.sang_x6.*obj.sang_x7).^2 + ... 288 | (obj.cang_x5.*obj.sang_x6.*obj.cang_x7 + obj.sang_x5.*obj.cang_x6.*obj.sang_x7).^2 + ... 289 | (obj.cang_x5.*obj.cang_x6.*obj.sang_x7 - obj.sang_x5.*obj.sang_x6.*obj.cang_x7).^2); 290 | 291 | obj.X1_next = obj.X1V + obj.h*((obj.J2-obj.J3)/obj.J1*obj.X2V.*obj.X3V + obj.U1V/obj.J1); 292 | obj.X2_next = obj.X2V + obj.h*((obj.J3-obj.J1)/obj.J2*obj.X3V.*obj.X1V + obj.U2V/obj.J2); 293 | obj.X3_next = obj.X3V + obj.h*((obj.J1-obj.J2)/obj.J3*obj.X1V.*obj.X2V + obj.U3V/obj.J3); 294 | %obj.X4_next = obj.X4 + obj.h*(0.5*(-obj.X1.*obj.X7 -obj.X2.*obj.X6 -obj.X3.*obj.X5)); 295 | obj.X5_next = (obj.cang_x5.*obj.cang_x6.*obj.sang_x7 - obj.sang_x5.*obj.sang_x6.*obj.cang_x7) + ... 296 | obj.h*(0.5*(obj.X2V.*(obj.sang_x5.*obj.cang_x6.*obj.cang_x7 - obj.cang_x5.*obj.sang_x6.*obj.sang_x7) ... 297 | -obj.X1V.*(obj.cang_x5.*obj.sang_x6.*obj.cang_x7 + obj.sang_x5.*obj.cang_x6.*obj.sang_x7) ... 298 | +obj.X3V.*x4)); 299 | 300 | obj.X6_next = (obj.cang_x5.*obj.sang_x6.*obj.cang_x7 + obj.sang_x5.*obj.cang_x6.*obj.sang_x7) + ... 301 | obj.h*(0.5*(-obj.X3V.*(obj.sang_x5.*obj.cang_x6.*obj.cang_x7 - obj.cang_x5.*obj.sang_x6.*obj.sang_x7) ... 302 | +obj.X1V.*(obj.cang_x5.*obj.cang_x6.*obj.sang_x7 - obj.sang_x5.*obj.sang_x6.*obj.cang_x7) ... 303 | +obj.X2V.*x4)); 304 | 305 | obj.X7_next = (obj.sang_x5.*obj.cang_x6.*obj.cang_x7 - obj.cang_x5.*obj.sang_x6.*obj.sang_x7) + ... 306 | obj.h*(0.5*(obj.X3V.*(obj.cang_x5.*obj.sang_x6.*obj.cang_x7 + obj.sang_x5.*obj.cang_x6.*obj.sang_x7) ... 307 | -obj.X2V.*(obj.cang_x5.*obj.cang_x6.*obj.sang_x7 - obj.sang_x5.*obj.sang_x6.*obj.cang_x7) ... 308 | +obj.X1V.*x4)); 309 | 310 | %x4_next 311 | x4 = x4 + obj.h*(0.5*(-obj.X1V.*(obj.sang_x5.*obj.cang_x6.*obj.cang_x7 - obj.cang_x5.*obj.sang_x6.*obj.sang_x7) ... 312 | -obj.X2V.*(obj.cang_x5.*obj.sang_x6.*obj.cang_x7 + obj.sang_x5.*obj.cang_x6.*obj.sang_x7) ... 313 | -obj.X3V.*(obj.cang_x5.*obj.cang_x6.*obj.sang_x7 - obj.sang_x5.*obj.sang_x6.*obj.cang_x7))); 314 | % 315 | % 316 | size_X5 = size(x4); %will be used in reshaping yaw, pitch, roll new to original n-D matrix 317 | 318 | x4 = x4(:); 319 | obj.X5_next = obj.X5_next(:); 320 | obj.X6_next = obj.X6_next(:); 321 | obj.X7_next = obj.X7_next(:); 322 | Qsquared_sum = sqrt(x4.^2 + obj.X5_next.^2 + obj.X6_next.^2 + obj.X7_next.^2); 323 | 324 | x4 = x4./Qsquared_sum; 325 | obj.X5_next = obj.X5_next./Qsquared_sum; 326 | obj.X6_next = obj.X6_next./Qsquared_sum; 327 | obj.X7_next = obj.X7_next./Qsquared_sum; 328 | 329 | r1 = atan2( 2.*(obj.X5_next.*obj.X6_next + x4.*obj.X7_next), ... 330 | x4.^2 + obj.X5_next.^2 - obj.X6_next.^2 - obj.X7_next.^2 ); 331 | r2 = asin( -2.*(obj.X5_next.*obj.X7_next - x4.*obj.X6_next) ); 332 | r3 = atan2( 2.*(obj.X6_next.*obj.X7_next + x4.*obj.X5_next), ... 333 | x4.^2 - obj.X5_next.^2 - obj.X6_next.^2 + obj.X7_next.^2 ); 334 | 335 | obj.X5_next = reshape(r1, size_X5); 336 | obj.X6_next = reshape(r2, size_X5); 337 | obj.X7_next = reshape(r3, size_X5); 338 | 339 | 340 | end 341 | 342 | function linear_control_response(spacecraft, X0, T_final, dt) 343 | if nargin < 2 344 | %sample initial state 345 | X0 = [1;1;1;... 346 | -0.0346073883029131;-0.346079245680143;0.343470774514906;0.872387133925326]; 347 | T_final = 1000; 348 | dt = 0.01; 349 | end 350 | 351 | 352 | N = T_final/dt; 353 | U = [-0.01; 0; 0.01]; 354 | X(:,1) = X0; 355 | % qc = [1, 0, 0, 0;... 356 | % 0, 1, 0, 0;... 357 | % 0, 0, 1, 0;... 358 | % 0, 0, 0, 1]; % q command (at origin, is equal to I(4x4) ) 359 | K = [0.5, 0, 0;... 360 | 0, 0.4, 0; 361 | 0, 0, 0.5]; 362 | C = [3, 0, 0;... 363 | 0, 3, 0; 364 | 0, 0, 4]; 365 | % keyboard; 366 | tic 367 | for k_stage=1:N 368 | % qe = qc*q; 369 | qe = X(5:7, k_stage); 370 | q = X(4:7, k_stage); 371 | w = X(1:3, k_stage); 372 | U(:,k_stage) = -K*qe - C*w; 373 | X(:,k_stage+1) = next_stage_states(spacecraft, [w',q'], U(:,k_stage)', dt); 374 | end 375 | q_squared_sum = X(4,:).^2 + X(5,:).^2 + X(6,:).^2 + X(7,:).^2; %check quaternions 376 | %print time and error 377 | % note: quaternions deviation from (sum(Q.^2) = 1) at T_final is a measure of error in ode solver 378 | fprintf(... 379 | 'Done - Time elapsed for caculations: %f - states max error: %.5g\n',... 380 | toc, sqrt((q_squared_sum(end) - 1))) 381 | 382 | time_v = linspace(0, T_final, N); %plot time vector 383 | % plot controls 384 | figure 385 | plot(time_v, U(1,:),'--') 386 | hold on 387 | plot(time_v, U(2,:),'--') 388 | plot(time_v, U(3,:),'--') 389 | grid on 390 | %plot states 391 | for n_state = 1:7 392 | plot(time_v, X(n_state, 1:end-1)) 393 | end 394 | legend('u1','u2','u3','x1','x2','x3','x4','x5','x6','x7') 395 | xlabel('time (s)') 396 | end 397 | 398 | function [X1_dot,X2_dot,X3_dot,... 399 | X5_dot,X6_dot,X7_dot] = spacecraft_dynamics(obj,x1,x2,x3,x4,x5,x6,u1,u2,u3) 400 | %returns the derivatives x_dot = f(X,u) 401 | %J is assumed to be diagonal, J12 = J23 = ... = 0 402 | X1_dot = (obj.J2-obj.J3)/obj.J1*x2.*x3 + u1/obj.J1; 403 | X2_dot = (obj.J3-obj.J1)/obj.J2*x3.*x1 + u2/obj.J2; 404 | X3_dot = (obj.J1-obj.J2)/obj.J3*x1.*x2 + u3/obj.J3; 405 | %X4_dot = 0.5*(-x1.*x7 -x2.*x6 -x3.*x5); 406 | X5_dot = 0.5*(x2.*x7 -x1.*x6 +x3.*x4); 407 | X6_dot = 0.5*(-x3.*x7 +x1.*x5 +x2.*x4); 408 | X7_dot = 0.5*(x3.*x6 -x2.*x5 +x1.*x4); 409 | end 410 | 411 | function X_dot = spacecraft_dynamics_list(obj, X,U) 412 | %returns the derivatives x_dot = f(X,u) 413 | X_dot = zeros(size(X)); 414 | x1 = X(:,1); 415 | x2 = X(:,2); 416 | x3 = X(:,3); 417 | x4 = X(:,4); 418 | x5 = X(:,5); 419 | x6 = X(:,6); 420 | x7 = X(:,7); 421 | u1 = U(:,1); 422 | u2 = U(:,2); 423 | u3 = U(:,3); 424 | %J is assumed to be diagonal, J12 = J23 = ... = 0 425 | X_dot(:,1) = (obj.J2-obj.J3)/obj.J1*x2.*x3 + u1/obj.J1; 426 | X_dot(:,2) = (obj.J3-obj.J1)/obj.J2*x3.*x1 + u2/obj.J2; 427 | X_dot(:,3) = (obj.J1-obj.J2)/obj.J3*x1.*x2 + u3/obj.J3; 428 | X_dot(:,4) = 0.5*(-x1.*x7 -x2.*x6 -x3.*x5); 429 | X_dot(:,5) = 0.5*(x2.*x7 -x1.*x6 +x3.*x4); 430 | X_dot(:,6) = 0.5*(-x3.*x7 +x1.*x5 +x2.*x4); 431 | X_dot(:,7) = 0.5*(x3.*x6 -x2.*x5 +x1.*x4); 432 | end 433 | 434 | function X2 = next_stage_states(spacecraft, X1, U, h) 435 | %calculates next stage (k+1) states 436 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 437 | %first order taylor expansion 438 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 439 | 440 | % Runge-Kutta - 4th order 441 | % h = dt; 442 | k1 = spacecraft_dynamics_list(spacecraft, X1 , U); 443 | k2 = spacecraft_dynamics_list(spacecraft, (X1 + k1*h/2), U); 444 | k3 = spacecraft_dynamics_list(spacecraft, (X1 + k2*h/2), U); 445 | k4 = spacecraft_dynamics_list(spacecraft, (X1 + k3*h), U); 446 | 447 | X2 = X1 + h*(k1 + 2*k2 + 2*k3 + k4); 448 | 449 | end 450 | 451 | function [sr_5, sr_6, sr_7] = mesh_quaternion(this) 452 | obj.s_yaw = linspace(deg2rad(this.yaw_min), deg2rad(this.yaw_max), this.n_mesh_q); 453 | obj.s_pitch = linspace(deg2rad(this.pitch_min), deg2rad(this.pitch_max), this.n_mesh_q); 454 | obj.s_roll = linspace(deg2rad(this.roll_min), deg2rad(this.roll_max), this.n_mesh_q); 455 | 456 | angles = [s_yaw(:) s_pitch(:) s_roll(:)]; 457 | 458 | cang = cos( angles/2 ); 459 | sang = sin( angles/2 ); 460 | 461 | sr_5 = cang(:,1).*sang(:,2).*cang(:,3) + sang(:,1).*cang(:,2).*sang(:,3); 462 | sr_6 = cang(:,1).*cang(:,2).*sang(:,3) - sang(:,1).*sang(:,2).*cang(:,3); 463 | sr_7 = cang(:,1).*cang(:,2).*cang(:,3) + sang(:,1).*sang(:,2).*sang(:,3); 464 | sr_5 = sr_5'; 465 | sr_6 = sr_6'; 466 | sr_7 = sr_7'; 467 | 468 | end 469 | 470 | function reshape_states(obj) 471 | n_mesh_w1 = length(obj.sr_1); 472 | n_mesh_w2 = length(obj.sr_2); 473 | n_mesh_w3 = length(obj.sr_3); 474 | 475 | obj.X1V = reshape(obj.sr_1, [n_mesh_w1 1]); 476 | obj.X2V = reshape(obj.sr_2, [1 n_mesh_w2]); 477 | obj.X3V = reshape(obj.sr_3, [1 1 n_mesh_w3]); 478 | 479 | n_mesh_yaw = length(obj.s_yaw); 480 | obj.cang_x5 = reshape( cos( obj.s_yaw/2 ), [1 1 1 n_mesh_yaw 1 1]); 481 | obj.sang_x5 = reshape( sin( obj.s_yaw/2 ), [1 1 1 n_mesh_yaw 1 1]); 482 | 483 | n_mesh_pitch = length(obj.s_pitch); 484 | obj.cang_x6 = reshape( cos( obj.s_pitch/2 ), [1 1 1 1 n_mesh_pitch 1]); 485 | obj.sang_x6 = reshape( sin( obj.s_pitch/2 ), [1 1 1 1 n_mesh_pitch 1]); 486 | 487 | n_mesh_roll = length(obj.s_roll); 488 | obj.cang_x7 = reshape( cos( obj.s_roll/2 ), [1 1 1 1 1 n_mesh_roll]); 489 | obj.sang_x7 = reshape( sin( obj.s_roll/2 ), [1 1 1 1 1 n_mesh_roll]); 490 | 491 | n_mesh_u = length(obj.U_vector); 492 | obj.U1V = reshape(obj.U_vector, [1 1 1 1 1 1 n_mesh_u]); 493 | obj.U2V = reshape(obj.U_vector, [1 1 1 1 1 1 1 n_mesh_u]); 494 | obj.U3V = reshape(obj.U_vector, [1 1 1 1 1 1 1 1 n_mesh_u]); 495 | end 496 | 497 | end 498 | 499 | end 500 | 501 | -------------------------------------------------------------------------------- /attitude-control/Solver_attitude.m: -------------------------------------------------------------------------------- 1 | classdef Solver_attitude < handle 2 | %SOLVER_ATTITUDE Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | N % number of stages 7 | InertiaM % Moment of Inertia Matrix 8 | J1 % element 1 of Inertia Matrix 9 | J2 % ... 10 | J3 % .. 11 | size_state_mat 12 | dim_U1 % dimension number of U, after the 7 state variables 13 | dim_U2 14 | dim_U3 15 | U_vector % values that are applied as control inputs 16 | % big data 17 | %J_next_states_opt single % optimum values of J will be stored in this matrix and recalled each stage 18 | J_current_state_fix single % values of cost to reach next stage, always fixed in Time Invariant system 19 | F % gridded Interpolant holding values of "J_next_states_opt" 20 | %obsolete, use X1V,etc... instead 21 | % X1 single % (s+c)-dimensional grid of state 1 22 | % X2 single 23 | % X3 single 24 | % X4 single 25 | % X5 single 26 | % X6 single 27 | % X7 single 28 | % U1 single % (s+c)-dimensional grid of control input 1 29 | % U2 single 30 | % U3 single 31 | X1_next single % (s+c)-dim grid of next state-1 32 | X2_next single 33 | X3_next single 34 | % X4_next single 35 | X4_next single 36 | X5_next single 37 | X6_next single 38 | 39 | U1_Opt % (s-dim x k) grid of optimal values U* for every stage and states 40 | U2_Opt 41 | U3_Opt 42 | %-- ranges for grid generation 43 | w_min 44 | w_max 45 | n_mesh_w 46 | n_mesh_t % for simplified dyn 47 | yaw_min 48 | yaw_max 49 | pitch_min 50 | pitch_max 51 | roll_min 52 | roll_max 53 | n_mesh_q 54 | sr_1 % state 1 range vector for grid and interpolant generation 55 | sr_2 % " 56 | sr_3 % " 57 | % sr_4 % " 58 | s_yaw % state 5 59 | s_pitch % " 6 60 | s_roll % " 7 61 | cr_1 % control input 1 range vector for grid generation 62 | cr_2 63 | cr_3 64 | 65 | T_final 66 | h %time step for discrete time system 67 | N_stage 68 | 69 | Q1 70 | Q2 71 | Q3 72 | Q4 73 | Q5 74 | Q6 75 | R1 76 | R2 77 | R3 78 | 79 | Qt1 %for simplified dyn 80 | Qt2 81 | Qt3 82 | 83 | X1V single 84 | X2V single 85 | X3V single 86 | cang_x4 single 87 | sang_x4 single 88 | cang_x5 single 89 | sang_x5 single 90 | cang_x6 single 91 | sang_x6 single 92 | cang_x7 single 93 | sang_x7 single 94 | U1V single 95 | U2V single 96 | U3V single 97 | 98 | defaultX0 % default value for initial states if not explicitly specified 99 | end 100 | 101 | methods 102 | %Constructor 103 | function this = Solver_attitude() 104 | % states are [w1; w2; w3; q1; q2; q3; q4] 105 | if nargin < 1 106 | w_min = -deg2rad(50); 107 | w_max = -deg2rad(-50); 108 | n_mesh_w = 1000; 109 | this.yaw_min = -30; %angles 110 | this.yaw_max = 30; 111 | this.pitch_min = -20; 112 | this.pitch_max = 20; 113 | this.roll_min = -35; 114 | this.roll_max = 35; 115 | this.n_mesh_q = 10; 116 | this.n_mesh_t = 300; %for simplified dyn 117 | 118 | inertia(1,1) = 0.02836 + 0.00016; 119 | inertia(2,1) = 0.026817 + 0.00150; 120 | inertia(3,1) = 0.023 + 0.00150; 121 | inertia(4,1) = -0.0000837; 122 | inertia(5,1) = 0.000014; 123 | inertia(6,1) = -0.00029; 124 | this.InertiaM = [inertia(1,1) inertia(4,1) inertia(5,1);... 125 | inertia(4,1) inertia(2,1) inertia(6,1);... 126 | inertia(5,1) inertia(6,1) inertia(3,1)]; 127 | 128 | this.Q1 = 6; 129 | this.Q2 = 6; 130 | this.Q3 = 6; 131 | 132 | this.Q4 = 6; %quaternions, thetas 133 | this.Q5 = 6; 134 | this.Q6 = 6; 135 | this.R1 = 4; 136 | this.R2 = 4; 137 | this.R3 = 4; 138 | 139 | this.Qt1 = this.Q4; 140 | this.Qt2 = this.Q5; 141 | this.Qt3 = this.Q6; 142 | 143 | this.T_final = 30; 144 | this.h = 0.005; 145 | end 146 | this.w_min = w_min; 147 | this.w_max = w_max; 148 | this.n_mesh_w = n_mesh_w; 149 | this.N_stage = this.T_final/this.h; 150 | 151 | if(~isinteger( this.N_stage)) 152 | this.N_stage = ceil(this.N_stage); 153 | this.T_final = this.h*this.N_stage; 154 | warning('T_final is not a factor of h (dt), increasing T_final to %.2f\n',this.T_final) 155 | end 156 | 157 | % q0 = [0;0;0.0174524064372835;0.999847695156391]; %angle2quat(0,0,roll = deg2rad(2)) 158 | 159 | %angle2quat(deg2rad(5),deg2rad(10),deg2rad(-9)) 160 | q0 = [0.0501511024391496;0.0833950587800888;-0.0818761044636256;0.991880252153991]; 161 | 162 | this.defaultX0 = [0;0;0;... 163 | ... %quaternions equal to quat(deg2rad(-10), deg2rad(20), deg2rad(-15)) 164 | q0];%0.999660006156261;0.00841930262082080;0.0176013597667272;0.0172968080698774]; 165 | 166 | 167 | this.J1 = this.InertiaM(1); 168 | this.J2 = this.InertiaM(5); 169 | this.J3 = this.InertiaM(9); 170 | 171 | this.dim_U1 = 7; 172 | this.dim_U2 = 8; 173 | this.dim_U3 = 9; 174 | this.U_vector = [-0.11 0 0.11]; 175 | length_U = length(this.U_vector); 176 | %Preallocation and mesh generation 177 | this.sr_1 = linspace(w_min, w_max, n_mesh_w); 178 | this.sr_2 = linspace(w_min, w_max, n_mesh_w); 179 | this.sr_3 = linspace(w_min, w_max, n_mesh_w); 180 | 181 | this.s_yaw = linspace(deg2rad(this.yaw_min), deg2rad(this.yaw_max), this.n_mesh_q); 182 | this.s_pitch = linspace(deg2rad(this.pitch_min), deg2rad(this.pitch_max), this.n_mesh_q); 183 | this.s_roll = linspace(deg2rad(this.roll_min), deg2rad(this.roll_max), this.n_mesh_q); 184 | 185 | this.size_state_mat = [n_mesh_w,n_mesh_w,n_mesh_w,... 186 | this.n_mesh_q,this.n_mesh_q,this.n_mesh_q]; 187 | 188 | if(0) %preallocation for system that is not simplified 189 | this.U1_Opt = zeros([this.size_state_mat],'uint8'); 190 | this.U2_Opt = zeros([this.size_state_mat,length_U],'uint8'); 191 | this.U3_Opt = zeros([this.size_state_mat,length_U,length_U],'uint8'); 192 | end 193 | end 194 | 195 | %Calculation of optimal matrices 196 | function simplified_run(obj) 197 | 198 | %% mesh generation 199 | s_w1 = linspace(obj.w_min, obj.w_max, obj.n_mesh_w); 200 | s_w2 = linspace(obj.w_min, obj.w_max, obj.n_mesh_w); 201 | s_w3 = linspace(obj.w_min, obj.w_max, obj.n_mesh_w); 202 | 203 | s_t1 = linspace(deg2rad(obj.yaw_min), deg2rad(obj.yaw_max), obj.n_mesh_t); 204 | s_t2 = linspace(deg2rad(obj.pitch_min), deg2rad(obj.pitch_max), obj.n_mesh_t); 205 | s_t3 = linspace(deg2rad(obj.roll_min), deg2rad(obj.roll_max), obj.n_mesh_t); 206 | 207 | %% initialization 208 | %states, controls 209 | [X1,X4,U1] = ndgrid(s_w1,s_t1,obj.U_vector); 210 | [X2,X5,U2] = ndgrid(s_w2,s_t2,obj.U_vector); 211 | [X3,X6,U3] = ndgrid(s_w3,s_t3,obj.U_vector); 212 | 213 | %u_opt 214 | size_Umat = [obj.n_mesh_w, obj.n_mesh_t]; 215 | obj.U1_Opt = zeros(size_Umat,'single'); 216 | obj.U2_Opt = zeros(size_Umat,'single'); 217 | obj.U3_Opt = zeros(size_Umat,'single'); 218 | 219 | %J matrices, current and optimal next stage 220 | J_current = @(w,theta,U,Qw,Qt,R)(Qw * w.^2 + Qt * theta.^2 + R*U.^2); 221 | J_current_1 = J_current(X1,X4,U1,obj.Q1,obj.Qt1,obj.R1); 222 | F1 = griddedInterpolant({s_w1,s_t1},zeros(obj.n_mesh_w,obj.n_mesh_t),'linear'); 223 | 224 | J_current_2 = J_current(X2,X5,U2,obj.Q2,obj.Qt2,obj.R2); 225 | F2 = griddedInterpolant({s_w2,s_t2},zeros(obj.n_mesh_w,obj.n_mesh_t),'linear'); 226 | 227 | J_current_3 = J_current(X3,X6,U3,obj.Q3,obj.Qt3,obj.R3); 228 | F3 = griddedInterpolant({s_w3,s_t3},zeros(obj.n_mesh_w,obj.n_mesh_t),'linear'); 229 | % 230 | % 231 | [w1_next,t1_next] = next_stage_states_simplified(obj,X1, X4, U1, obj.J1, obj.h); 232 | [w2_next,t2_next] = next_stage_states_simplified(obj,X2, X5, U2, obj.J2, obj.h); 233 | [w3_next,t3_next] = next_stage_states_simplified(obj,X3, X6, U3, obj.J3, obj.h); 234 | % keyboard 235 | whandle = waitbar(0,'Calculation in Progress...'); 236 | for k_s = obj.N_stage-1:-1:1 237 | tic 238 | % C = F1(w1_next,t1_next); 239 | [F1.Values, U1_idx] = min( J_current_1 + F1(w1_next,t1_next), [], 3); 240 | [F2.Values, U2_idx] = min( J_current_2 + F2(w2_next,t2_next), [], 3); 241 | [F3.Values, U3_idx] = min( J_current_3 + F3(w3_next,t3_next), [], 3); 242 | 243 | 244 | % QQ = U1_idx; 245 | waitbar( 1 - k_s/obj.N_stage, whandle); 246 | fprintf('step %d - %f seconds\n', k_s, toc) 247 | end 248 | %set U* Optimal 249 | obj.U1_Opt = griddedInterpolant({s_w1,s_t1}, obj.U_vector(U1_idx),'nearest'); 250 | obj.U2_Opt = griddedInterpolant({s_w2,s_t2}, obj.U_vector(U2_idx),'nearest'); 251 | obj.U3_Opt = griddedInterpolant({s_w3,s_t3}, obj.U_vector(U3_idx),'nearest'); 252 | close(whandle) 253 | fprintf('stage calculation complete... cleaning up...\n') 254 | % clear J_current_1 J_current_2 J_current_3 F1 F2 F3 X1 X2 X3 X4 X5 X6 U1 U2 U3 ... 255 | % w1_next w2_next w3_next t1_next t2_next t3_next 256 | fprintf('...Done!\n') 257 | 258 | 259 | end 260 | 261 | function run(obj) 262 | obj.reshape_states(); 263 | % calculate cost to reach next stage 264 | %obj.J_current_state_fix = g_D(obj); 265 | fprintf('calculating fixed cost matrix...\n') 266 | calculate_J_current_state_fix_shaped(obj); 267 | % Calculate and store J*NN = h(xi(N)) for all x(N) 268 | obj.F = griddedInterpolant(... 269 | {obj.sr_1, obj.sr_2, obj.sr_3, obj.s_yaw, obj.s_pitch, obj.s_roll}, ... 270 | zeros(obj.size_state_mat,'single'),'linear'); 271 | 272 | % 273 | fprintf('calculating next stage states...\n') 274 | calculate_states_next(obj); 275 | 276 | % 277 | %% obj.stage_J_star = H*X; 278 | %% 279 | fprintf('beginning stage calculation...\n') 280 | for k_s=obj.N_stage-1:-1:1 281 | tic 282 | calculate_J_U_opt_state_M(obj, k_s); 283 | fprintf('step %d - %f seconds\n', k_s, toc) 284 | %stop criteria (e.g. dFi(X) < tol, etc...) can be added here 285 | % id1 = this.sr_1 == 286 | % 287 | end 288 | %% final Ui_Opts 289 | % do NOT reverse the order of assignments 290 | obj.U3_Opt = obj.U3_Opt(obj.U2_Opt(obj.U1_Opt)); 291 | obj.U2_Opt = obj.U2_Opt(obj.U1_Opt); 292 | obj.U1_Opt = obj.U1_Opt; 293 | %report 294 | fprintf('stage calculation complete... cleaning up\n') 295 | clear_up(obj) 296 | obj.U1_Opt = single(obj.U_vector(obj.U1_Opt)); 297 | obj.U2_Opt = single(obj.U_vector(obj.U2_Opt)); 298 | obj.U3_Opt = single(obj.U_vector(obj.U3_Opt)); 299 | fprintf('...Done!\n') 300 | end 301 | 302 | function [X1,X2,X3,X4,X5,X6,X7] = a_D_M(obj, X1,X2,X3,X4,X5,X6,X7,U1,U2,U3) 303 | 304 | 305 | end 306 | 307 | function calculate_J_current_state_fix(obj) 308 | obj.J_current_state_fix = obj.Q1*obj.X1.^2 + obj.Q2*obj.X2.^2 + ... 309 | obj.Q3*obj.X3.^2 + ... % obj.Q4*obj.X4.^2 + ... 310 | obj.Q5*obj.X5.^2 + obj.Q6*obj.X6.^2 + ... 311 | obj.Q7*obj.X7.^2 + obj.R1*obj.U1.^2 + ... 312 | obj.R2*obj.U2.^2 + obj.R3*obj.U3.^2; 313 | end 314 | 315 | function calculate_J_current_state_fix_shaped(obj) 316 | obj.J_current_state_fix = obj.Q1*obj.X1V.^2 + obj.Q2*obj.X2V.^2 + ... 317 | obj.Q3*obj.X3V.^2 + ... % obj.Q4*obj.X4.^2 + ... 318 | obj.Q4*(obj.sang_x4.*obj.cang_x5.*obj.cang_x6 - obj.cang_x4.*obj.sang_x5.*obj.sang_x6).^2 + ... 319 | obj.Q5*(obj.cang_x4.*obj.sang_x5.*obj.cang_x6 + obj.sang_x4.*obj.cang_x5.*obj.sang_x6).^2 + ... 320 | obj.Q6*(obj.cang_x4.*obj.cang_x5.*obj.sang_x6 - obj.sang_x4.*obj.sang_x5.*obj.cang_x6).^2 + ... 321 | obj.R1*obj.U1V.^2 + obj.R2*obj.U2V.^2 + obj.R3*obj.U3V.^2; 322 | %% 323 | %q's are defined as follows: 324 | %x4 = q1 as defined in Kirk Control Theory, q4 by MATLAB 325 | %definition 326 | % (obj.sang_x4.*obj.cang_x5.*obj.cang_x6 - obj.cang_x4.*obj.sang_x5.*obj.sang_x6) 327 | 328 | %x5 = q2 as defined in Kirk Control Theory, q3 by MATLAB 329 | %definition 330 | % (obj.cang_x4.*obj.sang_x5.*obj.cang_x6 + obj.sang_x4.*obj.cang_x5.*obj.sang_x6) 331 | 332 | %x6 = q3 as defined in Kirk Control Theory, q2 by MATLAB 333 | %definition 334 | %%(obj.cang_x4.*obj.cang_x5.*obj.sang_x6 - obj.sang_x4.*obj.sang_x5.*obj.cang_x6) 335 | 336 | %x7 = q4 as defined in Kirk Control Theory, q1 by MATLAB 337 | %definition 338 | %(obj.cang_x4.*obj.cang_x5.*obj.cang_x6 + obj.sang_x4.*obj.sang_x5.*obj.sang_x6) 339 | 340 | %% 341 | 342 | end 343 | 344 | function calculate_states_next(obj) 345 | %calculates next stage (k+1) states 346 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 347 | 348 | % Runge-Kutta - 4th order 349 | % h = dt; 350 | % [obj.X1_next, obj.X2_next, obj.X3_next, obj.X4_next,... 351 | % obj.X5_next, obj.X6_next, obj.X7_next] = ... 352 | % spacecraft_dynamics(spacecraft, obj.X1, obj.X2, obj.X3, obj.X4, obj.X5, obj.X6,... 353 | % obj.X7, obj.U1, obj.U2, obj.U3); %X2 = k1 354 | % 355 | % [obj.X1_next, obj.X2_next, obj.X3_next, obj.X4_next,... 356 | % obj.X5_next, obj.X6_next, obj.X7_next] = ... 357 | % spacecraft_dynamics(spacecraft, obj.X1 + obj.X1_next*h/2 , obj.X2 + obj.X2_next*h/2,... 358 | % obj.X3 + obj.X3_next*h/2 , obj.X4 + obj.X4_next*h/2 , obj.X5 + obj.X5_next*h/2 ,... 359 | % obj.X6 + obj.X6_next*h/2 ,... 360 | % obj.X7 + obj.X7_next*h/2 , obj.U1, obj.U2, obj.U3); % k = k2 361 | % 362 | % k = spacecraft_dynamics(spacecraft, (X1 + X2*h/2), U); 363 | % X2 = X2 + 2*k; % X2 = k1 + 2*k2 364 | % k = spacecraft_dynamics(spacecraft, (X1 + k*h/2), U); % k = k3 365 | % X2 = X2 + 2*k; % X2 = k1 + 2*k2 + 2*k3 366 | % k = spacecraft_dynamics(spacecraft, (X1 + k*h), U); % k = k4 367 | % X2 = X2 + k; 368 | % X2 = X1 + h/6*(X2); 369 | 370 | 371 | %% taylor expansion 372 | spacecraft_dynamics_taylor_estimate(obj); 373 | % 374 | end 375 | 376 | function J = g_D(obj,X1,X2,X3,X5,X6,X7,U1,U2,U3) 377 | J = obj.Q1*X1.^2 + obj.Q2*X2.^2 + ... 378 | obj.Q3*X3.^2 + ... %obj.Q4*X4.^2 + ... 379 | obj.Q5*X5.^2 + obj.Q6*X6.^2 + ... 380 | obj.Q7*X7.^2 + obj.R1*U1.^2 + ... 381 | obj.R2*U2.^2 + obj.R3*U3.^2; 382 | end 383 | 384 | function calculate_J_U_opt_state_M(obj) 385 | 386 | %% CAUTION: this interpolant is only valid for Xmesh 387 | %find J final for each state and control (X,U) and add it to next state 388 | %optimum J* 389 | % nq = obj.n_mesh_q; 390 | % nw = obj.n_mesh_w; 391 | % nu = length(obj.U_vector); 392 | % [val, U_ID3] = min( obj.J_current_state_fix + ... 393 | % F(repmat(obj.X1_next,[1 1 1 nq nq nq 1 nu nu]),... %X1_next size = nw x nw x nw x 1 x 1 x 1 x nu 394 | % repmat(obj.X2_next,[1 1 1 nq nq nq nu 1 nu]),... %X2_next size = nw x nw x nw x 1 x 1 x 1 x 1 x nu 395 | % repmat(obj.X3_next,[1 1 1 nq nq nq nu nu 1]),... %X3_next size = nw x nw x nw x 1 x 1 x 1 x 1 x 1 x nu 396 | % repmat(obj.X5_next,[1 1 1 1 1 1 nu nu nu]),... %X5_next size = nw x nw x nw x nq x nq x nq 397 | % repmat(obj.X6_next,[1 1 1 1 1 1 nu nu nu]),... %X6_next size = nw x nw x nw x nq x nq x nq 398 | % repmat(obj.X7_next,[1 1 1 1 1 1 nu nu nu])), ... %X7_next size = nw x nw x nw x nq x nq x nq 399 | % [], obj.dim_U3); 400 | [val, obj.U3_Opt] = min( obj.J_current_state_fix + ... 401 | obj.F(obj.X1_next,... %X1_next size = nw x nw x nw x 1 x 1 x 1 x nu 402 | obj.X2_next,... %X2_next size = nw x nw x nw x 1 x 1 x 1 x 1 x nu 403 | obj.X3_next,... %X3_next size = nw x nw x nw x 1 x 1 x 1 x 1 x 1 x nu 404 | obj.X4_next,... %X4_next size = nw x nw x nw x nq x nq x nq 405 | obj.X5_next,... %X5_next size = nw x nw x nw x nq x nq x nq 406 | obj.X6_next), ... %X6_next size = nw x nw x nw x nq x nq x nq 407 | [], obj.dim_U3); 408 | [val, obj.U2_Opt] = min( val, [], obj.dim_U2); 409 | [obj.F.Values, obj.U1_Opt] = min( val, [], obj.dim_U1); 410 | 411 | end 412 | 413 | function spacecraft_dynamics_taylor_estimate(obj) 414 | %returns the derivatives x_dot = f(X,u) 415 | %J is assumed to be diagonal, J12 = J23 = ... = 0 416 | % x4 = (1 - (q1.^2 + ... 417 | % q2.^2 + ... 418 | % q3.^2)).^0.5; 419 | x7 = (1 - ( (obj.sang_x4.*obj.cang_x5.*obj.cang_x6 - obj.cang_x4.*obj.sang_x5.*obj.sang_x6).^2 + ... 420 | (obj.cang_x4.*obj.sang_x5.*obj.cang_x6 + obj.sang_x4.*obj.cang_x5.*obj.sang_x6).^2 + ... 421 | (obj.cang_x4.*obj.cang_x5.*obj.sang_x6 - obj.sang_x4.*obj.sang_x5.*obj.cang_x6).^2) ).^0.5; 422 | 423 | obj.X1_next = obj.X1V + obj.h*((obj.J2-obj.J3)/obj.J1*obj.X2V.*obj.X3V + obj.U1V/obj.J1); 424 | obj.X2_next = obj.X2V + obj.h*((obj.J3-obj.J1)/obj.J2*obj.X3V.*obj.X1V + obj.U2V/obj.J2); 425 | obj.X3_next = obj.X3V + obj.h*((obj.J1-obj.J2)/obj.J3*obj.X1V.*obj.X2V + obj.U3V/obj.J3); 426 | %obj.X4_next = obj.X4 + obj.h*(0.5*(-obj.X1.*obj.X7 -obj.X2.*obj.X6 -obj.X3.*obj.X5)); 427 | % simplified: 428 | % obj.X4_next = x4 + ... 429 | % obj.h*(0.5*(obj.X3V.* x5 ... 430 | % -obj.X2V.*x6 ... 431 | % +obj.X1V.*x7)); 432 | % 433 | % obj.X5_next = x5 + ... 434 | % obj.h*(0.5*(-obj.X3V.*x4 ... 435 | % +obj.X1V.*x6 ... 436 | % +obj.X2V.*x7)); 437 | % 438 | % obj.X6_next = x6 + ... 439 | % obj.h*(0.5*(obj.X2V.*x4 ... 440 | % -obj.X1V.*x5 ... 441 | % +obj.X3V.*x7)); 442 | % 443 | % %x4_next 444 | % x7 = x7 + obj.h*(0.5*(-obj.X1V.*x4 ... 445 | % -obj.X2V.*x5 ... 446 | % -obj.X3V.*x6 )); 447 | 448 | % % expanded(actual): 449 | obj.X4_next = (obj.sang_x4.*obj.cang_x5.*obj.cang_x6 - obj.cang_x4.*obj.sang_x5.*obj.sang_x6) + ... 450 | obj.h*(0.5*(obj.X3V.* (obj.cang_x4.*obj.sang_x5.*obj.cang_x6 + obj.sang_x4.*obj.cang_x5.*obj.sang_x6) ... 451 | -obj.X2V.*(obj.cang_x4.*obj.cang_x5.*obj.sang_x6 - obj.sang_x4.*obj.sang_x5.*obj.cang_x6) ... 452 | +obj.X1V.*x7)); 453 | 454 | obj.X5_next = (obj.cang_x4.*obj.sang_x5.*obj.cang_x6 + obj.sang_x4.*obj.cang_x5.*obj.sang_x6) + ... 455 | obj.h*(0.5*(-obj.X3V.*(obj.sang_x4.*obj.cang_x5.*obj.cang_x6 - obj.cang_x4.*obj.sang_x5.*obj.sang_x6) ... 456 | +obj.X1V.*(obj.cang_x4.*obj.cang_x5.*obj.sang_x6 - obj.sang_x4.*obj.sang_x5.*obj.cang_x6) ... 457 | +obj.X2V.*x7)); 458 | 459 | obj.X6_next = (obj.cang_x4.*obj.cang_x5.*obj.sang_x6 - obj.sang_x4.*obj.sang_x5.*obj.cang_x6) + ... 460 | obj.h*(0.5*(obj.X2V.*(obj.sang_x4.*obj.cang_x5.*obj.cang_x6 - obj.cang_x4.*obj.sang_x5.*obj.sang_x6) ... 461 | -obj.X1V.*(obj.cang_x4.*obj.sang_x5.*obj.cang_x6 + obj.sang_x4.*obj.cang_x5.*obj.sang_x6) ... 462 | +obj.X3V.*x7)); 463 | 464 | %x4_next 465 | x7 = x7 + obj.h*(0.5*(-obj.X1V.*(obj.sang_x4.*obj.cang_x5.*obj.cang_x6 - obj.cang_x4.*obj.sang_x5.*obj.sang_x6) ... 466 | -obj.X2V.*(obj.cang_x4.*obj.sang_x5.*obj.cang_x6 + obj.sang_x4.*obj.cang_x5.*obj.sang_x6) ... 467 | -obj.X3V.*(obj.cang_x4.*obj.cang_x5.*obj.sang_x6 - obj.sang_x4.*obj.sang_x5.*obj.cang_x6) )); 468 | % 469 | % 470 | size_X5 = size(x7); %will be used in reshaping yaw, pitch, roll new to original n-D matrix 471 | 472 | obj.X4_next = obj.X4_next(:); 473 | obj.X5_next = obj.X5_next(:); 474 | obj.X6_next = obj.X6_next(:); 475 | x7 = x7(:); 476 | 477 | Qsquared_sum = sqrt( obj.X4_next.^2 + obj.X5_next.^2 + obj.X6_next.^2 + x7.^2 ); 478 | 479 | 480 | obj.X4_next = obj.X4_next./Qsquared_sum; 481 | obj.X5_next = obj.X5_next./Qsquared_sum; 482 | obj.X6_next = obj.X6_next./Qsquared_sum; 483 | x7 = x7./Qsquared_sum; 484 | 485 | r1_yaw_next = atan2( 2.*(obj.X6_next.*obj.X5_next + x7.*obj.X4_next),... 486 | x7.^2 + obj.X6_next.^2 - obj.X5_next.^2 - obj.X4_next.^2 ); 487 | r2_pitch_next = asin( -2.*(obj.X6_next.*obj.X4_next - x7.*obj.X5_next) ); 488 | r3_roll_next = atan2(2.*(obj.X5_next.*obj.X4_next + x7.*obj.X6_next), ... 489 | x7.^2 - obj.X6_next.^2 - obj.X5_next.^2 + obj.X4_next.^2); 490 | 491 | obj.X4_next = reshape(r1_yaw_next, size_X5); 492 | obj.X5_next = reshape(r2_pitch_next, size_X5); 493 | obj.X6_next = reshape(r3_roll_next, size_X5); 494 | %temporary repmat%%%%%%%%%%%%%%%%%%%%%%%%%%% 495 | nq = obj.n_mesh_q; 496 | % nw = obj.n_mesh_w; 497 | nu = length(obj.U_vector); 498 | 499 | obj.X1_next = repmat(obj.X1_next,[1 1 1 nq nq nq 1 nu nu]); %X1_next size = nw x nw x nw x 1 x 1 x 1 x nu 500 | obj.X2_next = repmat(obj.X2_next,[1 1 1 nq nq nq nu 1 nu]); %X2_next size = nw x nw x nw x 1 x 1 x 1 x 1 x nu 501 | obj.X3_next = repmat(obj.X3_next,[1 1 1 nq nq nq nu nu 1]); %X3_next size = nw x nw x nw x 1 x 1 x 1 x 1 x 1 x nu 502 | obj.X4_next = repmat(obj.X4_next,[1 1 1 1 1 1 nu nu nu]); %X4_next size = nw x nw x nw x nq x nq x nq 503 | obj.X5_next = repmat(obj.X5_next,[1 1 1 1 1 1 nu nu nu]); %X5_next size = nw x nw x nw x nq x nq x nq 504 | obj.X6_next = repmat(obj.X6_next,[1 1 1 1 1 1 nu nu nu]); %X6_next size = nw x nw x nw x nq x nq x nq 505 | %%%%%%%%%%%% 506 | end 507 | 508 | function linear_control_response(spacecraft, X0, T_final, dt) 509 | %example: S.linear_control_response(S.defaultX0, 100,1e-3 510 | 511 | if nargin < 2 512 | %sample initial state 513 | X0 = spacecraft.defaultX0; 514 | T_final = spacecraft.T_final ; 515 | dt = spacecraft.h; 516 | end 517 | 518 | N = T_final/dt; 519 | U = spacecraft.U_vector'; 520 | X(:,1) = X0; 521 | qc = [1, 0, 0, 0;... 522 | 0, 1, 0, 0;... 523 | 0, 0, 1, 0;... 524 | 0, 0, 0, 1]; % q command (at origin, is equal to I(4x4) ) 525 | K = [0.2, 0, 0;... 526 | 0, 0.2, 0; 527 | 0, 0, 0.2]; 528 | C = [1, 0, 0;... 529 | 0, 1, 0; 530 | 0, 0, 1]; 531 | % keyboard; 532 | tic 533 | for k_stage=1:N 534 | % qe = qc*q; 535 | q = X(4:7, k_stage); 536 | qe = qc*q; 537 | w = X(1:3, k_stage); 538 | U(:,k_stage) = -K*qe(1:3) - C*w; 539 | X(:,k_stage+1) = next_stage_states(spacecraft, [w',q'], U(:,k_stage)', dt); 540 | [x_yaw,x_pitch,x_roll] = quat2angle([X(7,k_stage),X(6,k_stage),X(5,k_stage),X(4,k_stage)]); 541 | XANGLES(:,k_stage) = [x_yaw;x_pitch;x_roll]; 542 | end 543 | q_squared_sum = sqrt(X(4,:).^2 + X(5,:).^2 + X(6,:).^2 + X(7,:).^2); %check quaternions 544 | %print time and error 545 | % note: quaternions deviation from (sum(Q.^2) = 1) at T_final is a measure of error in ode solver 546 | fprintf(... 547 | 'Done - Time elapsed for caculations: %f - states max error: %.5g\n',... 548 | toc, sqrt((q_squared_sum(end) - 1))) 549 | 550 | time_v = linspace(0, T_final, N); %plot time vector 551 | % plot controls 552 | figure 553 | plot(time_v, U(1,:),'--') 554 | hold on 555 | plot(time_v, U(2,:),'--') 556 | plot(time_v, U(3,:),'--') 557 | grid on 558 | title('Control Inputs') 559 | legend('u1','u2','u3') 560 | 561 | %plot states 562 | figure 563 | hold on 564 | grid on 565 | for n_state = [1 2 3] 566 | plot(time_v, X(n_state, 1:end-1).*180/pi) 567 | end 568 | legend('\omega_1','\omega_2','\omega_3') 569 | xlabel('time (s)') 570 | ylabel('\omega (deg/sec)') 571 | 572 | figure 573 | hold on 574 | grid on 575 | for n_state = [4 5 6 7] 576 | plot(time_v, X(n_state, 1:end-1)) 577 | end 578 | legend('q1','q2','q3','q4') 579 | xlabel('time (s)') 580 | 581 | figure 582 | hold on 583 | grid on 584 | for n_state = [1 2 3] 585 | plot(time_v, XANGLES(n_state, :).*180/pi) 586 | end 587 | legend('\theta_1','\theta_2','\theta_3') 588 | xlabel('time (s)') 589 | ylabel('\theta (deg)') 590 | 591 | end 592 | 593 | function [X1_dot,X2_dot,X3_dot,... 594 | X5_dot,X6_dot,X7_dot] = spacecraft_dynamics(obj,x1,x2,x3,x4,x5,x6,u1,u2,u3) 595 | %returns the derivatives x_dot = f(X,u) 596 | %J is assumed to be diagonal, J12 = J23 = ... = 0 597 | 598 | end 599 | 600 | function X_dot = spacecraft_dynamics_list(obj, X,U) 601 | %returns the derivatives x_dot = f(X,u) 602 | % FIXED q~ convention 603 | X_dot = zeros(size(X)); 604 | x1 = X(:,1); 605 | x2 = X(:,2); 606 | x3 = X(:,3); 607 | x4 = X(:,4); 608 | x5 = X(:,5); 609 | x6 = X(:,6); 610 | x7 = X(:,7); 611 | u1 = U(:,1); 612 | u2 = U(:,2); 613 | u3 = U(:,3); 614 | %J is assumed to be diagonal, J12 = J23 = ... = 0 615 | X_dot(:,1) = (obj.J2-obj.J3)/obj.J1*x2.*x3 + u1/obj.J1; 616 | X_dot(:,2) = (obj.J3-obj.J1)/obj.J2*x3.*x1 + u2/obj.J2; 617 | X_dot(:,3) = (obj.J1-obj.J2)/obj.J3*x1.*x2 + u3/obj.J3; 618 | X_dot(:,4) = 0.5*(x3.*x5 -x2.*x6 +x1.*x7); 619 | X_dot(:,5) = 0.5*(-x3.*x4 +x1.*x6 +x2.*x7); 620 | X_dot(:,6) = 0.5*(x2.*x4 -x1.*x5 +x3.*x7); 621 | X_dot(:,7) = 0.5*(-x1.*x4 -x2.*x5 -x3.*x6); 622 | end 623 | 624 | %% functions for simplified dynamics 625 | function [w_next,t_next] = next_stage_states_simplified(obj,W, T, U, J, h) 626 | w_next = RK4_w(obj,W, U, J, h); 627 | t_next = RK4_t(obj,T, W, h); 628 | end 629 | 630 | function W2 = RK4_w(obj,w, U, J, h) 631 | %calculates next stage (k+1) states 632 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 633 | %first order taylor expansion 634 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 635 | 636 | % Runge-Kutta - 4th order 637 | % h = dt; 638 | k1 = wdynamics(obj,w , U, J); 639 | k2 = wdynamics(obj,(w + k1*h/2), U, J); 640 | k3 = wdynamics(obj,(w + k2*h/2), U, J); 641 | k4 = wdynamics(obj,(w + k3*h), U, J); 642 | 643 | W2 = w + h*(k1 + 2*k2 + 2*k3 + k4)/6; 644 | end 645 | 646 | function T2 = RK4_t(obj,T1, W1, h) 647 | %calculates next stage (k+1) states 648 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 649 | %first order taylor expansion 650 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 651 | 652 | % Runge-Kutta - 4th order 653 | % h = dt; 654 | k1 = tdynamics(obj,W1); 655 | k2 = tdynamics(obj,(W1 + k1*h/2)); 656 | k3 = tdynamics(obj,(W1 + k2*h/2)); 657 | k4 = tdynamics(obj,(W1 + k3*h)); 658 | 659 | T2 = T1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 660 | end 661 | 662 | function w_dot = wdynamics(~,~, U, J) 663 | w_dot = U/J; 664 | end 665 | function t_dot = tdynamics(~,w) 666 | t_dot = w; 667 | end 668 | %% end of simplified dynamics functions 669 | 670 | function X2 = next_stage_states(spacecraft, X1, U, h, mode) 671 | if nargin < 5 672 | mode = 'RK4'; 673 | end 674 | %calculates next stage (k+1) states 675 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 676 | %first order taylor expansion 677 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 678 | 679 | % Runge-Kutta - 4th order 680 | % h = dt; 681 | k1 = spacecraft_dynamics_list(spacecraft, X1 , U); 682 | if (strcmp(mode,'RK4')) 683 | k2 = spacecraft_dynamics_list(spacecraft, (X1 + k1*h/2), U); 684 | k3 = spacecraft_dynamics_list(spacecraft, (X1 + k2*h/2), U); 685 | k4 = spacecraft_dynamics_list(spacecraft, (X1 + k3*h), U); 686 | 687 | 688 | X2 = X1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 689 | 690 | elseif(strcmp(mode,'taylor')) 691 | X2 = X1 + h*k1; 692 | end 693 | q_sqsum = sqrt(X2(4).^2 + X2(5).^2 + X2(6).^2 + X2(7).^2); 694 | X2(4:7) = X2(4:7)./q_sqsum; 695 | 696 | end 697 | 698 | function [sr_5, sr_6, sr_7] = mesh_quaternion(this) 699 | obj.s_yaw = linspace(deg2rad(this.yaw_min), deg2rad(this.yaw_max), this.n_mesh_q); 700 | obj.s_pitch = linspace(deg2rad(this.pitch_min), deg2rad(this.pitch_max), this.n_mesh_q); 701 | obj.s_roll = linspace(deg2rad(this.roll_min), deg2rad(this.roll_max), this.n_mesh_q); 702 | 703 | angles = [s_yaw(:) s_pitch(:) s_roll(:)]; 704 | 705 | cang = cos( angles/2 ); 706 | sang = sin( angles/2 ); 707 | 708 | % sr_5 = cang(:,1).*sang(:,2).*cang(:,3) + sang(:,1).*cang(:,2).*sang(:,3); 709 | % sr_6 = cang(:,1).*cang(:,2).*sang(:,3) - sang(:,1).*sang(:,2).*cang(:,3); 710 | % sr_7 = cang(:,1).*cang(:,2).*cang(:,3) + sang(:,1).*sang(:,2).*sang(:,3); 711 | sr_5 = sr_5'; 712 | sr_6 = sr_6'; 713 | sr_7 = sr_7'; 714 | 715 | end 716 | 717 | function reshape_states(obj) 718 | n_mesh_w1 = length(obj.sr_1); 719 | n_mesh_w2 = length(obj.sr_2); 720 | n_mesh_w3 = length(obj.sr_3); 721 | 722 | obj.X1V = reshape(obj.sr_1, [n_mesh_w1 1]); 723 | obj.X2V = reshape(obj.sr_2, [1 n_mesh_w2]); 724 | obj.X3V = reshape(obj.sr_3, [1 1 n_mesh_w3]); 725 | 726 | n_mesh_yaw = length(obj.s_yaw); 727 | obj.cang_x4 = reshape( cos( obj.s_yaw/2 ), [1 1 1 n_mesh_yaw 1 1]); 728 | obj.sang_x4 = reshape( sin( obj.s_yaw/2 ), [1 1 1 n_mesh_yaw 1 1]); 729 | 730 | n_mesh_pitch = length(obj.s_pitch); 731 | obj.cang_x5 = reshape( cos( obj.s_pitch/2 ), [1 1 1 1 n_mesh_pitch 1]); 732 | obj.sang_x5 = reshape( sin( obj.s_pitch/2 ), [1 1 1 1 n_mesh_pitch 1]); 733 | 734 | n_mesh_roll = length(obj.s_roll); 735 | obj.cang_x6 = reshape( cos( obj.s_roll/2 ), [1 1 1 1 1 n_mesh_roll]); 736 | obj.sang_x6 = reshape( sin( obj.s_roll/2 ), [1 1 1 1 1 n_mesh_roll]); 737 | 738 | n_mesh_u = length(obj.U_vector); 739 | obj.U1V = reshape(obj.U_vector, [1 1 1 1 1 1 n_mesh_u]); 740 | obj.U2V = reshape(obj.U_vector, [1 1 1 1 1 1 1 n_mesh_u]); 741 | obj.U3V = reshape(obj.U_vector, [1 1 1 1 1 1 1 1 n_mesh_u]); 742 | end 743 | 744 | function get_optimal_path(obj, X0, method) 745 | if nargin < 2 746 | X0 = obj.defaultX0; 747 | method = 'nearest'; 748 | end 749 | X = zeros(7,obj.N_stage); 750 | X(:,1) = X0; 751 | U = zeros(3,obj.N_stage); 752 | X_ANGLES = zeros(9,obj.N_stage); 753 | tic 754 | for k_stage=1:obj.N_stage-1 755 | % qe = qc*q; 756 | %% proper q input to quat2angle() 757 | [x_yaw,x_pitch,x_roll] = quat2angle([X(7,k_stage),X(6,k_stage),X(5,k_stage),X(4,k_stage)]); 758 | % X(4:7,k_stage) = angle2quat(x_yaw,x_pitch,x_roll)'; 759 | FU1 = griddedInterpolant({obj.sr_1, obj.sr_2, obj.sr_3, obj.s_yaw, obj.s_pitch, obj.s_roll},... 760 | obj.U1_Opt,method); 761 | FU2 = griddedInterpolant({obj.sr_1, obj.sr_2, obj.sr_3, obj.s_yaw, obj.s_pitch, obj.s_roll},... 762 | obj.U2_Opt,method); 763 | FU3 = griddedInterpolant({obj.sr_1, obj.sr_2, obj.sr_3, obj.s_yaw, obj.s_pitch, obj.s_roll},... 764 | obj.U3_Opt,method); 765 | 766 | U(1,k_stage) = FU1(... 767 | X(1,k_stage),X(2,k_stage),X(3,k_stage), ... 768 | x_yaw,x_pitch,x_roll); 769 | 770 | U(2,k_stage) = FU2(... 771 | X(1,k_stage),X(2,k_stage),X(3,k_stage), ... 772 | x_yaw,x_pitch,x_roll); 773 | 774 | U(3,k_stage) = FU3(... 775 | X(1,k_stage),X(2,k_stage),X(3,k_stage), ... 776 | x_yaw,x_pitch,x_roll); 777 | 778 | %uses taylor for next stage calculations 779 | X(:,k_stage+1) = next_stage_states(obj, X(:,k_stage)', U(:,k_stage)', obj.h, 'taylor'); 780 | 781 | X_ANGLES(:,k_stage) = [X(1,k_stage);X(2,k_stage);X(3,k_stage);... 782 | rad2deg(x_roll);rad2deg(x_pitch);rad2deg(x_yaw);... 783 | U(:,k_stage)]; 784 | end 785 | toc 786 | %% check vector values (should start from 0) 787 | 788 | v_plot = 0:obj.h:obj.T_final-obj.h; 789 | figure 790 | hold on 791 | for i=1:3 792 | plot(v_plot, U(i,:),'--') 793 | end 794 | legend('u1','u2','u3') 795 | grid on 796 | xlabel('time (s)') 797 | 798 | %plot w1,w2,w3 799 | figure 800 | hold on 801 | grid on 802 | for n_state = [1 2 3] 803 | plot(v_plot, X(n_state, :).*180/pi) 804 | end 805 | legend('\omega_1','\omega_2','\omega_3') 806 | xlabel('time (s)') 807 | ylabel('\omega (deg/sec)') 808 | 809 | 810 | %plot angles 811 | figure 812 | hold on 813 | for i=4:6 814 | plot(v_plot, X_ANGLES(i,:)) 815 | end 816 | title('rotation angles') 817 | legend('roll','pitch','yaw') 818 | grid on 819 | ylabel('deg') 820 | xlabel('time (s)') 821 | 822 | 823 | figure 824 | hold on 825 | for i=1:6 826 | plot(v_plot, X(i,1:end)) 827 | end 828 | title('states') 829 | legend('x1','x2','x3','x5','x6','x7') 830 | grid on 831 | xlabel('time (s)') 832 | 833 | end 834 | 835 | function get_optimal_path_simplified_testode45(obj) 836 | tic 837 | X0 = obj.defaultX0; 838 | FU1 = obj.U1_Opt; 839 | FU2 = obj.U2_Opt; 840 | FU3 = obj.U3_Opt; 841 | tspan = 0:obj.h:obj.T_final; 842 | X_ode45 = zeros(obj.N_stage, 7); 843 | X_ode45(1,:) = X0; 844 | for k_stage=1:obj.N_stage-1 845 | %determine U 846 | X_stage = X_ode45(k_stage,:); 847 | U1(1) = FU1(X_stage(1), 2*asin(X_stage(4))); 848 | U1(2) = FU2(X_stage(2), 2*asin(X_stage(5))); 849 | U1(3) = FU3(X_stage(3), 2*asin(X_stage(6))); 850 | % 851 | [~,X_temp] = ode45(@ode_eq,[tspan(k_stage), tspan(k_stage+1)], X_ode45(k_stage,:)); 852 | X_ode45(k_stage+1,:) = X_temp(end,:); 853 | end 854 | toc 855 | T_ode45 = tspan(1:end-1)'; 856 | [t_yaw,t_pitch,t_roll] = quat2angle([X_ode45(:,7),X_ode45(:,6),X_ode45(:,5),X_ode45(:,4)]); 857 | theta1_ode45 = t_yaw; 858 | theta2_ode45 = t_pitch; 859 | theta3_ode45 = t_roll; 860 | U1_ode45 = FU1(X_ode45(:,1), theta1_ode45); 861 | U2_ode45 = FU2(X_ode45(:,2), theta2_ode45); 862 | U3_ode45 = FU3(X_ode45(:,3), theta3_ode45); 863 | 864 | %% plot rotational speeds 865 | figure 866 | hold on 867 | grid on 868 | for n_state = [1 2 3] 869 | plot(T_ode45, X_ode45(:, n_state).*180/pi) 870 | end 871 | legend('\omega_1','\omega_2','\omega_3') 872 | xlabel('time (s)') 873 | ylabel('\omega (deg/sec)') 874 | 875 | %% plot angles 876 | figure 877 | hold on 878 | plot(T_ode45,theta1_ode45) 879 | plot(T_ode45,theta2_ode45) 880 | plot(T_ode45,theta3_ode45) 881 | 882 | title('rotation angles') 883 | legend('\theta_1','\theta_2','\theta_3') 884 | grid on 885 | xlabel('time (s)') 886 | 887 | %% plot controls 888 | figure 889 | plot(T_ode45, U1_ode45,'--') 890 | hold on 891 | plot(T_ode45, U2_ode45,'--') 892 | plot(T_ode45, U3_ode45,'--') 893 | grid on 894 | title('Control Inputs') 895 | legend('u1','u2','u3') 896 | 897 | %function declarations 898 | function x_dot = ode_eq(~,X1) 899 | 900 | x_dot = system_dynamics(X1, U1'); 901 | x_dot = x_dot'; 902 | function X_dot = system_dynamics(X, U) 903 | x1 = X(1); 904 | x2 = X(2); 905 | x3 = X(3); 906 | x4 = X(4); 907 | x5 = X(5); 908 | x6 = X(6); 909 | x7 = X(7); 910 | %InertiaM is a complete matrix 911 | w = [x1;x2;x3]; 912 | w_dot = obj.InertiaM\(U - cross(w, obj.InertiaM*w)); 913 | 914 | X_dot(1) = w_dot(1); 915 | X_dot(2) = w_dot(2); 916 | X_dot(3) = w_dot(3); 917 | 918 | X_dot(4) = 0.5*(x3.*x5 -x2.*x6 +x1.*x7); 919 | X_dot(5) = 0.5*(-x3.*x4 +x1.*x6 +x2.*x7); 920 | X_dot(6) = 0.5*(x2.*x4 -x1.*x5 +x3.*x7); 921 | X_dot(7) = 0.5*(-x1.*x4 -x2.*x5 -x3.*x6); 922 | end 923 | 924 | end 925 | end 926 | 927 | function clear_up(obj) 928 | % clears up RAM from big matrices that were used in calculations and are no longer needed 929 | obj.X2_next = []; 930 | obj.X1_next = []; 931 | obj.X3_next = []; 932 | obj.X4_next = []; 933 | obj.X5_next = []; 934 | obj.X6_next = []; 935 | obj.J_current_state_fix = []; 936 | obj.F = []; 937 | end 938 | end 939 | 940 | end 941 | 942 | -------------------------------------------------------------------------------- /attitude-control/test/test_dim_times.m: -------------------------------------------------------------------------------- 1 | %% 2 | x = [1,2] 3 | u1 = [3 0 -2]' 4 | u2 = reshape([-4 2 4],[1 1 3]) 5 | 6 | J1 = x + u1 + 3*u2 7 | 8 | %% 9 | [X,U1,U2] = ndgrid([3 0 -2],[1,2],[-4 2 4]) 10 | J2 = X + U1 + 3*U2 11 | %% 12 | 13 | J3_p = u1 + 3*u2 14 | 15 | %% 16 | 17 | yaw1 = [0,1,2] 18 | pitch1 = [1,1.5,2] 19 | roll1 = [-1,-2,5] 20 | 21 | %% n-D gridded implementation / uses memory to store grid 22 | [X1,X2,X5,X6,X7] = ndgrid(x,x,yaw1,pitch1,roll1); 23 | size_Q = size(X5); 24 | angles = [X5(:) X6(:) X7(:)]; 25 | 26 | cang = cos( angles/2 ); 27 | sang = sin( angles/2 ); 28 | %start test 29 | tic 30 | X5 = reshape( cang(:,1).*sang(:,2).*cang(:,3) + sang(:,1).*cang(:,2).*sang(:,3), size_Q); 31 | X6 = reshape( cang(:,1).*cang(:,2).*sang(:,3) - sang(:,1).*sang(:,2).*cang(:,3), size_Q); 32 | X7 = reshape( cang(:,1).*cang(:,2).*cang(:,3) + sang(:,1).*sang(:,2).*sang(:,3), size_Q); 33 | 34 | JQ1 = X1.^2 + 2*X2.^2 + X5*2 + X6*3 + X7*5; 35 | toc 36 | %end test 37 | 38 | %% vector implementation / saves memory 39 | cang_x5 = reshape( cos( yaw1/2 ), [1 1 3 1 1]); 40 | sang_x5 = reshape( sin( yaw1/2 ), [1 1 3 1 1]); 41 | 42 | cang_x6 = reshape( cos( pitch1/2 ), [1 1 1 3 1]); 43 | sang_x6 = reshape( sin( pitch1/2 ), [1 1 1 3 1]); 44 | 45 | cang_x7 = reshape( cos( roll1/2 ), [1 1 1 1 3]); 46 | sang_x7 = reshape( sin( roll1/2 ), [1 1 1 1 3]); 47 | X1V = reshape(x,[2 1 1 1 1]); 48 | X2V = reshape(x,[1 2 1 1 1]); 49 | %start test 50 | tic 51 | X5V = cang_x5.*sang_x6.*cang_x7 + sang_x5.*cang_x6.*sang_x7; 52 | X6V = cang_x5.*cang_x6.*sang_x7 - sang_x5.*sang_x6.*cang_x7; 53 | X7V = cang_x5.*cang_x6.*cang_x7 + sang_x5.*sang_x6.*sang_x7; 54 | JQ2 = X1V.^2 + 2*X2V.^2 + X5V*2 + X6V*3 + X7V*5; 55 | toc 56 | %end test 57 | %% Direct vector implementation / much less memory and time required 58 | %start test 59 | tic 60 | JQ3 = X1V.^2 + 2*X2V.^2 + ... 61 | (cang_x5.*sang_x6.*cang_x7 + sang_x5.*cang_x6.*sang_x7)*2 + ... 62 | (cang_x5.*cang_x6.*sang_x7 - sang_x5.*sang_x6.*cang_x7)*3 + ... 63 | (cang_x5.*cang_x6.*cang_x7 + sang_x5.*sang_x6.*sang_x7)*5; 64 | toc 65 | %end test 66 | -------------------------------------------------------------------------------- /attitude-control/test/test_min_ndim.m: -------------------------------------------------------------------------------- 1 | u1 = [3 -2 1]; 2 | u2 = [1 2 -2]; 3 | 4 | x1 = [1 2 3] 5 | [X1,U1,U2] = ndgrid(x1,u1,u2); 6 | J = X1.*U1.*U2; 7 | 8 | [val, id2] = min(J,[],3) 9 | [val, id1] = min(val,[],2) 10 | 11 | u2(id2(id1)) 12 | u1(id1) 13 | x.*u2(id2(id1)).*u1(id1) 14 | 15 | -------------------------------------------------------------------------------- /attitude-control/test/test_simplified.m: -------------------------------------------------------------------------------- 1 | function test_simplified() 2 | %% default parameters 3 | skip_calc = 0; 4 | simplified = 1; 5 | q0 = [0;0;0.0174524064372835;0.999847695156391]; %angle2quat(0,0,roll = deg2rad(2)) 6 | X0 = [0;0;0;... 7 | ... %quaternions equal to quat(deg2rad(-15), deg2rad(20), deg2rad(-10)) 8 | q0];%0.999660006156261;0.00841930262082080;0.0176013597667272;0.0172968080698774]; 9 | 10 | w_min = deg2rad(-0.7); 11 | w_max = deg2rad(0.7); 12 | n_mesh_w = 10; 13 | yaw_min = -5; %angles 14 | yaw_max = 5; 15 | pitch_min = -4; 16 | pitch_max = 4; 17 | roll_min = -5.5; 18 | roll_max = 5.5; 19 | n_mesh_t = 10; 20 | 21 | % u = linspace(-1, 0, 1000); 22 | % U_vector = [u(1:end-1), linspace(0, 1, 1000) ]; 23 | U_vector = [-0.01 0 0.01]; 24 | % n_mesh_u = length(U_vector); 25 | Qw1 = 6; 26 | Qw2 = 6; 27 | Qw3 = 6; 28 | Qt1 = 6; 29 | Qt2 = 6; 30 | Qt3 = 6; 31 | R1 = 0.1; 32 | R2 = 0.1; 33 | R3 = 0.1; 34 | 35 | T_final = 45; 36 | h = 0.01; 37 | N_stage = T_final/h; 38 | 39 | if(~isinteger( N_stage)) 40 | N_stage = ceil(N_stage); 41 | T_final = h*N_stage; 42 | warning('T_final is not a factor of h (dt), increasing T_final to %.2f\n',T_final) 43 | end 44 | 45 | 46 | J1 = 2; 47 | J2 = 2.5; 48 | J3 = 3; 49 | 50 | 51 | %% mesh generation 52 | s_w1 = linspace(w_min, w_max, n_mesh_w); 53 | s_w2 = linspace(w_min, w_max, n_mesh_w); 54 | s_w3 = linspace(w_min, w_max, n_mesh_w); 55 | 56 | s_t1 = linspace(deg2rad(yaw_min), deg2rad(yaw_max), n_mesh_t); 57 | s_t2 = linspace(deg2rad(pitch_min), deg2rad(pitch_max), n_mesh_t); 58 | s_t3 = linspace(deg2rad(roll_min), deg2rad(roll_max), n_mesh_t); 59 | 60 | 61 | %% initialization 62 | %states, controls 63 | [X1,X4,U1] = ndgrid(s_w1,s_t1,U_vector); 64 | [X2,X5,U2] = ndgrid(s_w2,s_t2,U_vector); 65 | [X3,X6,U3] = ndgrid(s_w3,s_t3,U_vector); 66 | 67 | %u_opt 68 | size_Umat = [n_mesh_w, n_mesh_t, N_stage]; 69 | U1_Opt = zeros(size_Umat,'single'); 70 | U2_Opt = zeros(size_Umat,'single'); 71 | U3_Opt = zeros(size_Umat,'single'); 72 | 73 | %J matrices, current and optimal next stage 74 | 75 | J_current_1 = J_current(X1,X4,U1,Qw1,Qt1,R1); 76 | F1 = griddedInterpolant({s_w1,s_t1},zeros(n_mesh_w,n_mesh_t),'linear'); 77 | 78 | J_current_2 = J_current(X2,X5,U2,Qw2,Qt2,R2); 79 | F2 = griddedInterpolant({s_w2,s_t2},zeros(n_mesh_w,n_mesh_t),'linear'); 80 | 81 | J_current_3 = J_current(X3,X6,U3,Qw3,Qt3,R3); 82 | F3 = griddedInterpolant({s_w3,s_t3},zeros(n_mesh_w,n_mesh_t),'linear'); 83 | % 84 | J11 = J_current_1(:,:,1); 85 | X11 = X1(:,:,1); 86 | X44 = X4(:,:,1); 87 | % 88 | [w1_next,t1_next] = next_stage_states(X1, X4, U1, J1, h); 89 | [w2_next,t2_next] = next_stage_states(X2, X5, U2, J2, h); 90 | [w3_next,t3_next] = next_stage_states(X3, X6, U3, J3, h); 91 | % keyboard 92 | %% loop 93 | if (skip_calc == 0 ) 94 | whandle = waitbar(0,'Calculation in Progress...'); 95 | for k_s = N_stage-1:-1:1 96 | tic 97 | % C = F1(w1_next,t1_next); 98 | [F1.Values, U1_idx] = min( J_current_1 + F1(w1_next,t1_next), [], 3); 99 | [F2.Values, U2_idx] = min( J_current_2 + F2(w2_next,t2_next), [], 3); 100 | [F3.Values, U3_idx] = min( J_current_3 + F3(w3_next,t3_next), [], 3); 101 | 102 | U1_Opt(:,:,k_s) = U_vector(U1_idx); 103 | U2_Opt(:,:,k_s) = U_vector(U2_idx); 104 | U3_Opt(:,:,k_s) = U_vector(U3_idx); 105 | % QQ = U1_idx; 106 | waitbar( 1 - k_s/N_stage, whandle); 107 | fprintf('step %d - %f seconds\n', k_s, toc) 108 | end 109 | close(whandle) 110 | fprintf('stage calculation complete... cleaning up...\n') 111 | % clear J_current_1 J_current_2 J_current_3 F1 F2 F3 X1 X2 X3 X4 X5 X6 U1 U2 U3 ... 112 | % w1_next w2_next w3_next t1_next t2_next t3_next 113 | fprintf('...Done!\n') 114 | else 115 | fprintf('Calculation skipped, load the required data\n') 116 | end 117 | 118 | keyboard 119 | 120 | %% Simulate results 121 | if(simplified == 1) 122 | %% test on Simplified system dynamics ----------------------------------------------------- 123 | X_Simplified = zeros(6, N_stage); 124 | [t3,t2,t1] = quat2angle([X0(7),X0(6),X0(5),X0(4)]); 125 | X_Simplified(:,1) = [0;0;0;t1;t2;t3]; 126 | U = zeros(3,N_stage); 127 | X_ANGLES = zeros(3,N_stage); 128 | tic 129 | for k_stage=1:N_stage-1 130 | t1 = X_Simplified(4,k_stage); 131 | t2 = X_Simplified(5,k_stage); 132 | t3 = X_Simplified(6,k_stage); 133 | % t3 = 2*asin(X(4,k_stage)); 134 | % t2 = 2*asin(X(5,k_stage)); 135 | % t1 = 2*asin(X(6,k_stage)); 136 | % t1,t2,t3 137 | FU1 = griddedInterpolant({s_w1,s_t1}, single(U1_Opt(:,:,k_stage)),'nearest'); 138 | FU2 = griddedInterpolant({s_w2,s_t2}, single(U2_Opt(:,:,k_stage)),'nearest'); 139 | FU3 = griddedInterpolant({s_w3,s_t3}, single(U3_Opt(:,:,k_stage)),'nearest'); 140 | 141 | U(1,k_stage) = FU1(X_Simplified(1,k_stage), t1); 142 | U(2,k_stage) = FU2(X_Simplified(2,k_stage), t2); 143 | U(3,k_stage) = FU3(X_Simplified(3,k_stage), t3); 144 | 145 | [w1_new,t1_new] = next_stage_states(X_Simplified(1,k_stage), t1, U(1,k_stage), J1, h); 146 | [w2_new,t2_new] = next_stage_states(X_Simplified(2,k_stage), t2, U(2,k_stage), J2, h); 147 | [w3_new,t3_new] = next_stage_states(X_Simplified(3,k_stage), t3, U(3,k_stage), J3, h); 148 | X_Simplified(:,k_stage+1) = [w1_new;w2_new;w3_new;t1_new;t2_new;t3_new]; 149 | 150 | X_ANGLES(:,k_stage) = [rad2deg(t1);rad2deg(t2);rad2deg(t3)]; 151 | end 152 | toc 153 | 154 | %% Plot Simplified System 155 | time_v = linspace(0, T_final, N_stage); %plot time vector 156 | % plot controls 157 | figure 158 | plot(time_v, U(1,:),'--') 159 | hold on 160 | plot(time_v, U(2,:),'--') 161 | plot(time_v, U(3,:),'--') 162 | grid on 163 | title('Control Inputs') 164 | legend('u1','u2','u3') 165 | 166 | %plot w 167 | figure 168 | hold on 169 | grid on 170 | for n_state = [1 2 3] 171 | plot(time_v, X_Simplified(n_state, :).*180/pi) 172 | end 173 | legend('\omega_1','\omega_2','\omega_3') 174 | xlabel('time (s)') 175 | ylabel('\omega (deg/sec)') 176 | 177 | figure 178 | hold on 179 | for i=1:3 180 | plot(time_v, X_ANGLES(i,:)) 181 | end 182 | title('rotation angles') 183 | legend('\theta_1','\theta_2','\theta_3') 184 | grid on 185 | xlabel('time (s)') 186 | 187 | 188 | %% test on REAL SYSTEM DYNAMICS ---------------------------------------------------------- 189 | else 190 | X = zeros(7, N_stage); 191 | X(:,1) = X0; 192 | U = zeros(3,N_stage); 193 | X_ANGLES = zeros(9,N_stage); 194 | tic 195 | for k_stage=1:N_stage-1 196 | [x_yaw,x_pitch,x_roll] = quat2angle([X(7,k_stage),X(6,k_stage),X(5,k_stage),X(4,k_stage)]); 197 | % t3 = x_roll; 198 | % t2 = x_pitch; 199 | % t1 = x_yaw; 200 | t1 = 2*asin(X(4,k_stage)); 201 | t2 = 2*asin(X(5,k_stage)); 202 | t3 = 2*asin(X(6,k_stage)); 203 | % t1,t2,t3 204 | FU1 = griddedInterpolant({s_w1,s_t1}, single(U1_Opt(:,:,1)),'nearest'); 205 | FU2 = griddedInterpolant({s_w2,s_t2}, single(U2_Opt(:,:,1)),'nearest'); 206 | FU3 = griddedInterpolant({s_w3,s_t3}, single(U3_Opt(:,:,1)),'nearest'); 207 | 208 | U(1,k_stage) = FU1(X(1,k_stage), t1); 209 | U(2,k_stage) = FU2(X(2,k_stage), t2); 210 | U(3,k_stage) = FU3(X(3,k_stage), t3); 211 | %% test on Real system dynamics 212 | 213 | X(:,k_stage+1) = real_system_dynamics(X(:,k_stage)', U(:,k_stage)', J1, J2, J3, h); 214 | 215 | X_ANGLES(:,k_stage) = [X(1,k_stage);X(2,k_stage);X(3,k_stage);... % w1,w2,w3 216 | rad2deg(t1);rad2deg(t2);rad2deg(t3);... % angles 217 | U(:,k_stage)]; % controls 218 | end 219 | toc 220 | 221 | %% Plot REAL SYSTEM 222 | time_v = linspace(0, T_final, N_stage); %plot time vector 223 | % plot controls 224 | figure 225 | plot(time_v, U(1,:),'--') 226 | hold on 227 | plot(time_v, U(2,:),'--') 228 | plot(time_v, U(3,:),'--') 229 | grid on 230 | title('Control Inputs') 231 | legend('u1','u2','u3') 232 | 233 | %plot states 234 | figure 235 | hold on 236 | grid on 237 | for n_state = [1 2 3] 238 | plot(time_v, X(n_state, :).*180/pi) 239 | end 240 | legend('\omega_1','\omega_2','\omega_3') 241 | xlabel('time (s)') 242 | ylabel('\omega (deg/sec)') 243 | 244 | figure 245 | hold on 246 | for i=4:6 247 | plot(time_v, X_ANGLES(i,:)) 248 | end 249 | title('rotation angles') 250 | legend('\theta_1','\theta_2','\theta_3') 251 | grid on 252 | xlabel('time (s)') 253 | 254 | 255 | figure 256 | hold on 257 | grid on 258 | for n_state = [4 5 6 7] 259 | plot(time_v, X(n_state, :)) 260 | end 261 | legend('q1','q2','q3','q4') 262 | xlabel('time (s)') 263 | 264 | end %end if 265 | keyboard 266 | keyboard 267 | keyboard 268 | keyboard 269 | 270 | function J_ = J_current(w,theta,U,Qw,Qt,R) 271 | J_ = (Qw * w.^2 + Qt * theta.^2 + R*U.^2); 272 | 273 | function [w_next,t_next] = next_stage_states(W, T, U, J, h) 274 | w_next = RK4_w(W, U, J, h); 275 | t_next = RK4_t(T, W, h); 276 | 277 | 278 | function W2 = RK4_w(w, U, J, h) 279 | %calculates next stage (k+1) states 280 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 281 | %first order taylor expansion 282 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 283 | 284 | % Runge-Kutta - 4th order 285 | % h = dt; 286 | k1 = wdynamics(w , U, J); 287 | k2 = wdynamics((w + k1*h/2), U, J); 288 | k3 = wdynamics((w + k2*h/2), U, J); 289 | k4 = wdynamics((w + k3*h), U, J); 290 | 291 | W2 = w + h*(k1 + 2*k2 + 2*k3 + k4)/6; 292 | 293 | 294 | function T2 = RK4_t(T1, W1, h) 295 | %calculates next stage (k+1) states 296 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 297 | %first order taylor expansion 298 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 299 | 300 | % Runge-Kutta - 4th order 301 | % h = dt; 302 | k1 = tdynamics(W1); 303 | k2 = tdynamics((W1 + k1*h/2)); 304 | k3 = tdynamics((W1 + k2*h/2)); 305 | k4 = tdynamics((W1 + k3*h)); 306 | 307 | T2 = T1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 308 | 309 | 310 | function w_dot = wdynamics(~, U, J) 311 | w_dot = U/J; 312 | 313 | function t_dot = tdynamics(w) 314 | t_dot = w; 315 | 316 | 317 | function X2 = real_system_dynamics(X1, U, J1, J2, J3, h) 318 | %calculates next stage (k+1) states 319 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 320 | %first order taylor expansion 321 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 322 | 323 | % Runge-Kutta - 4th order 324 | % h = dt; 325 | k1 = spacecraft_dynamics_list( X1 , U, J1, J2, J3); 326 | k2 = spacecraft_dynamics_list((X1 + k1*h/2), U, J1, J2, J3); 327 | k3 = spacecraft_dynamics_list((X1 + k2*h/2), U, J1, J2, J3); 328 | k4 = spacecraft_dynamics_list( (X1 + k3*h), U, J1, J2, J3); 329 | 330 | X2 = X1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 331 | q_sqsum = sqrt(X2(4)^2 + X2(5)^2 + X2(6)^2 + X2(7)^2); 332 | X2(4:7) = X2(4:7)/q_sqsum; 333 | 334 | 335 | function X_dot = spacecraft_dynamics_list(X, U, J1, J2, J3) 336 | %returns the derivatives x_dot = f(X,u) 337 | % FIXED q~ convention 338 | X_dot = zeros(size(X)); 339 | x1 = X(:,1); 340 | x2 = X(:,2); 341 | x3 = X(:,3); 342 | x4 = X(:,4); 343 | x5 = X(:,5); 344 | x6 = X(:,6); 345 | x7 = X(:,7); 346 | u1 = U(:,1); 347 | u2 = U(:,2); 348 | u3 = U(:,3); 349 | %J is assumed to be diagonal, J12 = J23 = ... = 0 350 | X_dot(:,1) = (J2-J3)/J1*x2.*x3 + u1/J1; 351 | X_dot(:,2) = (J3-J1)/J2*x3.*x1 + u2/J2; 352 | X_dot(:,3) = (J1-J2)/J3*x1.*x2 + u3/J3; 353 | X_dot(:,4) = 0.5*(x3.*x5 -x2.*x6 +x1.*x7); 354 | X_dot(:,5) = 0.5*(-x3.*x4 +x1.*x6 +x2.*x7); 355 | X_dot(:,6) = 0.5*(x2.*x4 -x1.*x5 +x3.*x7); 356 | X_dot(:,7) = 0.5*(-x1.*x4 -x2.*x5 -x3.*x6); 357 | -------------------------------------------------------------------------------- /attitude-control/test/test_simplified_20deg.m: -------------------------------------------------------------------------------- 1 | function test_simplified_20deg() 2 | %% default parameters 3 | skip_calc = 1; 4 | simplified = 0; 5 | q0 = [0;0;0.0174524064372835;0.999847695156391]; %angle2quat(0,0,roll = deg2rad(2)) 6 | X0 = [0;0;0;... 7 | ... %quaternions equal to quat(deg2rad(-15), deg2rad(20), deg2rad(-10)) 8 | q0];%0.999660006156261;0.00841930262082080;0.0176013597667272;0.0172968080698774]; 9 | 10 | w_min = deg2rad(-0.7); 11 | w_max = deg2rad(0.7); 12 | n_mesh_w = 10; 13 | yaw_min = -5; %angles 14 | yaw_max = 5; 15 | pitch_min = -4; 16 | pitch_max = 4; 17 | roll_min = -5.5; 18 | roll_max = 5.5; 19 | n_mesh_t = 10; 20 | 21 | % u = linspace(-1, 0, 1000); 22 | % U_vector = [u(1:end-1), linspace(0, 1, 1000) ]; 23 | U_vector = [-0.01 0 0.01]; 24 | % n_mesh_u = length(U_vector); 25 | Qw1 = 6; 26 | Qw2 = 6; 27 | Qw3 = 6; 28 | Qt1 = 6; 29 | Qt2 = 6; 30 | Qt3 = 6; 31 | R1 = 0.1; 32 | R2 = 0.1; 33 | R3 = 0.1; 34 | 35 | T_final = 45; 36 | h = 0.01; 37 | N_stage = T_final/h; 38 | 39 | if(~isinteger( N_stage)) 40 | N_stage = ceil(N_stage); 41 | T_final = h*N_stage; 42 | warning('T_final is not a factor of h (dt), increasing T_final to %.2f\n',T_final) 43 | end 44 | 45 | 46 | J1 = 2; 47 | J2 = 2.5; 48 | J3 = 3; 49 | 50 | 51 | %% mesh generation 52 | s_w1 = linspace(w_min, w_max, n_mesh_w); 53 | s_w2 = linspace(w_min, w_max, n_mesh_w); 54 | s_w3 = linspace(w_min, w_max, n_mesh_w); 55 | 56 | s_t1 = linspace(deg2rad(roll_min), deg2rad(roll_max), n_mesh_t); 57 | s_t2 = linspace(deg2rad(pitch_min), deg2rad(pitch_max), n_mesh_t); 58 | s_t3 = linspace(deg2rad(yaw_min), deg2rad(yaw_max), n_mesh_t); 59 | 60 | 61 | %% initialization 62 | %states, controls 63 | [X1,X4,U1] = ndgrid(s_w1,s_t1,U_vector); 64 | [X2,X5,U2] = ndgrid(s_w2,s_t2,U_vector); 65 | [X3,X6,U3] = ndgrid(s_w3,s_t3,U_vector); 66 | 67 | %u_opt 68 | size_Umat = [n_mesh_w, n_mesh_t, N_stage]; 69 | U1_Opt = zeros(size_Umat,'single'); 70 | U2_Opt = zeros(size_Umat,'single'); 71 | U3_Opt = zeros(size_Umat,'single'); 72 | 73 | %J matrices, current and optimal next stage 74 | 75 | J_current_1 = J_current(X1,X4,U1,Qw1,Qt1,R1); 76 | F1 = griddedInterpolant({s_w1,s_t1},zeros(n_mesh_w,n_mesh_t),'linear'); 77 | 78 | J_current_2 = J_current(X2,X5,U2,Qw2,Qt2,R2); 79 | F2 = griddedInterpolant({s_w2,s_t2},zeros(n_mesh_w,n_mesh_t),'linear'); 80 | 81 | J_current_3 = J_current(X3,X6,U3,Qw3,Qt3,R3); 82 | F3 = griddedInterpolant({s_w3,s_t3},zeros(n_mesh_w,n_mesh_t),'linear'); 83 | % 84 | J11 = J_current_1(:,:,1); 85 | X11 = X1(:,:,1); 86 | X44 = X4(:,:,1); 87 | % 88 | [w1_next,t1_next] = next_stage_states(X1, X4, U1, J1, h); 89 | [w2_next,t2_next] = next_stage_states(X2, X5, U2, J2, h); 90 | [w3_next,t3_next] = next_stage_states(X3, X6, U3, J3, h); 91 | % keyboard 92 | %% loop 93 | if (skip_calc == 0 ) 94 | whandle = waitbar(0,'Calculation in Progress...'); 95 | for k_s = N_stage-1:-1:1 96 | tic 97 | % C = F1(w1_next,t1_next); 98 | [F1.Values, U1_idx] = min( J_current_1 + F1(w1_next,t1_next), [], 3); 99 | [F2.Values, U2_idx] = min( J_current_2 + F2(w2_next,t2_next), [], 3); 100 | [F3.Values, U3_idx] = min( J_current_3 + F3(w3_next,t3_next), [], 3); 101 | 102 | U1_Opt(:,:,k_s) = U_vector(U1_idx); 103 | U2_Opt(:,:,k_s) = U_vector(U2_idx); 104 | U3_Opt(:,:,k_s) = U_vector(U3_idx); 105 | % QQ = U1_idx; 106 | waitbar( 1 - k_s/N_stage, whandle); 107 | fprintf('step %d - %f seconds\n', k_s, toc) 108 | end 109 | close(whandle) 110 | fprintf('stage calculation complete... cleaning up...\n') 111 | % clear J_current_1 J_current_2 J_current_3 F1 F2 F3 X1 X2 X3 X4 X5 X6 U1 U2 U3 ... 112 | % w1_next w2_next w3_next t1_next t2_next t3_next 113 | fprintf('...Done!\n') 114 | else 115 | load('simplified-test-20deg.mat') 116 | fprintf('Calculation skipped, loaded the required data\n') 117 | end 118 | 119 | keyboard 120 | 121 | %% Simulate results 122 | if(simplified == 1) 123 | %% test on Simplified system dynamics ----------------------------------------------------- 124 | X_Simplified = zeros(6, N_stage); 125 | [t3,t2,t1] = quat2angle([X0(7),X0(6),X0(5),X0(4)]); 126 | X_Simplified(:,1) = [0;0;0;t1;t2;t3]; 127 | U = zeros(3,N_stage); 128 | X_ANGLES = zeros(3,N_stage); 129 | tic 130 | for k_stage=1:N_stage-1 131 | t1 = X_Simplified(4,k_stage); 132 | t2 = X_Simplified(5,k_stage); 133 | t3 = X_Simplified(6,k_stage); 134 | % t3 = 2*asin(X(4,k_stage)); 135 | % t2 = 2*asin(X(5,k_stage)); 136 | % t1 = 2*asin(X(6,k_stage)); 137 | % t1,t2,t3 138 | FU1 = griddedInterpolant({s_w1,s_t1}, single(U1_Opt(:,:,k_stage)),'nearest'); 139 | FU2 = griddedInterpolant({s_w2,s_t2}, single(U2_Opt(:,:,k_stage)),'nearest'); 140 | FU3 = griddedInterpolant({s_w3,s_t3}, single(U3_Opt(:,:,k_stage)),'nearest'); 141 | 142 | U(1,k_stage) = FU1(X_Simplified(1,k_stage), t1); 143 | U(2,k_stage) = FU2(X_Simplified(2,k_stage), t2); 144 | U(3,k_stage) = FU3(X_Simplified(3,k_stage), t3); 145 | 146 | [w1_new,t1_new] = next_stage_states(X_Simplified(1,k_stage), t1, U(1,k_stage), J1, h); 147 | [w2_new,t2_new] = next_stage_states(X_Simplified(2,k_stage), t2, U(2,k_stage), J2, h); 148 | [w3_new,t3_new] = next_stage_states(X_Simplified(3,k_stage), t3, U(3,k_stage), J3, h); 149 | X_Simplified(:,k_stage+1) = [w1_new;w2_new;w3_new;t1_new;t2_new;t3_new]; 150 | 151 | X_ANGLES(:,k_stage) = [rad2deg(t1);rad2deg(t2);rad2deg(t3)]; 152 | end 153 | toc 154 | 155 | %% Plot Simplified System 156 | time_v = linspace(0, T_final, N_stage); %plot time vector 157 | % plot controls 158 | figure 159 | plot(time_v, U(1,:),'--') 160 | hold on 161 | plot(time_v, U(2,:),'--') 162 | plot(time_v, U(3,:),'--') 163 | grid on 164 | title('Control Inputs') 165 | legend('u1','u2','u3') 166 | 167 | %plot w 168 | figure 169 | hold on 170 | grid on 171 | for n_state = [1 2 3] 172 | plot(time_v, X_Simplified(n_state, :).*180/pi) 173 | end 174 | legend('\omega_1','\omega_2','\omega_3') 175 | xlabel('time (s)') 176 | ylabel('\omega (deg/sec)') 177 | 178 | figure 179 | hold on 180 | for i=1:3 181 | plot(time_v, X_ANGLES(i,:)) 182 | end 183 | title('rotation angles') 184 | legend('\theta_1','\theta_2','\theta_3') 185 | grid on 186 | xlabel('time (s)') 187 | 188 | 189 | %% test on REAL SYSTEM DYNAMICS ---------------------------------------------------------- 190 | else 191 | X = zeros(7, N_stage); 192 | X(:,1) = X0; 193 | U = zeros(3,N_stage); 194 | X_ANGLES = zeros(9,N_stage); 195 | tic 196 | for k_stage=1:N_stage-1 197 | % t3 = x_roll; 198 | % t2 = x_pitch; 199 | % t1 = x_yaw; 200 | x_roll = 2*asin(X(4,k_stage)); 201 | x_pitch = 2*asin(X(5,k_stage)); 202 | x_yaw = 2*asin(X(6,k_stage)); 203 | % 204 | t1 = x_roll; 205 | t2 = x_pitch; 206 | t3 = x_yaw; 207 | 208 | % t1,t2,t3 209 | FU1 = griddedInterpolant({s_w1,s_t1}, single(U1_Opt(:,:,1)),'nearest'); 210 | FU2 = griddedInterpolant({s_w2,s_t2}, single(U2_Opt(:,:,1)),'nearest'); 211 | FU3 = griddedInterpolant({s_w3,s_t3}, single(U3_Opt(:,:,1)),'nearest'); 212 | 213 | U(1,k_stage) = FU1(X(1,k_stage), t1); 214 | U(2,k_stage) = FU2(X(2,k_stage), t2); 215 | U(3,k_stage) = FU3(X(3,k_stage), t3); 216 | %% test on Real system dynamics 217 | 218 | X(:,k_stage+1) = real_system_dynamics(X(:,k_stage)', U(:,k_stage)', J1, J2, J3, h); 219 | [t1,t2,t3] = quat2angle([X(7,k_stage),X(6,k_stage),X(5,k_stage),X(4,k_stage)]); 220 | 221 | X_ANGLES(:,k_stage) = [X(1,k_stage);X(2,k_stage);X(3,k_stage);... % w1,w2,w3 222 | rad2deg(t1);rad2deg(t2);rad2deg(t3);... % angles 223 | U(:,k_stage)]; % controls 224 | end 225 | toc 226 | 227 | %% Plot REAL SYSTEM 228 | time_v = linspace(0, T_final, N_stage); %plot time vector 229 | % plot controls 230 | figure 231 | plot(time_v, U(1,:),'--') 232 | hold on 233 | plot(time_v, U(2,:),'--') 234 | plot(time_v, U(3,:),'--') 235 | grid on 236 | title('Control Inputs') 237 | legend('u1','u2','u3') 238 | 239 | %plot states 240 | figure 241 | hold on 242 | grid on 243 | for n_state = [1 2 3] 244 | plot(time_v, X(n_state, :).*180/pi) 245 | end 246 | legend('\omega_1','\omega_2','\omega_3') 247 | xlabel('time (s)') 248 | ylabel('\omega (deg/sec)') 249 | 250 | figure 251 | hold on 252 | for i=4:6 253 | plot(time_v, X_ANGLES(i,:)) 254 | end 255 | title('rotation angles') 256 | legend('\theta_1','\theta_2','\theta_3') 257 | grid on 258 | xlabel('time (s)') 259 | 260 | 261 | figure 262 | hold on 263 | grid on 264 | for n_state = [4 5 6 7] 265 | plot(time_v, X(n_state, :)) 266 | end 267 | legend('q1','q2','q3','q4') 268 | xlabel('time (s)') 269 | 270 | end %end if 271 | 272 | keyboard 273 | 274 | 275 | k_s = 1:N_stage-2; 276 | keyboard 277 | this.X1_mesh = X1(:,:,1); 278 | this.X2_mesh = X4(:,:,1); 279 | % 280 | this.u_star = U1_Opt; 281 | % 282 | if length(k_s) == 1 283 | figure 284 | plot3( this.X1_mesh, this.X2_mesh, this.u_star(:,:,k_s) ) 285 | else 286 | figure 287 | p = mesh(this.X1_mesh, this.X2_mesh, this.u_star(:,:,k_s(1)) ); 288 | colormap winter 289 | % not allowing axis limits to change automatically 290 | axis manual 291 | for i=2:length(k_s) 292 | k_temp = k_s(i); 293 | p.ZData = this.u_star(:,:,k_s(i)); 294 | title(['Stage ',num2str(k_temp)]); 295 | pause(0.001) 296 | end 297 | end 298 | keyboard 299 | 300 | function J_ = J_current(w,theta,U,Qw,Qt,R) 301 | J_ = (Qw * w.^2 + Qt * theta.^2 + R*U.^2); 302 | 303 | function [w_next,t_next] = next_stage_states(W, T, U, J, h) 304 | w_next = RK4_w(W, U, J, h); 305 | t_next = RK4_t(T, W, h); 306 | 307 | 308 | function W2 = RK4_w(w, U, J, h) 309 | %calculates next stage (k+1) states 310 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 311 | %first order taylor expansion 312 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 313 | 314 | % Runge-Kutta - 4th order 315 | % h = dt; 316 | k1 = wdynamics(w , U, J); 317 | k2 = wdynamics((w + k1*h/2), U, J); 318 | k3 = wdynamics((w + k2*h/2), U, J); 319 | k4 = wdynamics((w + k3*h), U, J); 320 | 321 | W2 = w + h*(k1 + 2*k2 + 2*k3 + k4)/6; 322 | 323 | 324 | function T2 = RK4_t(T1, W1, h) 325 | %calculates next stage (k+1) states 326 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 327 | %first order taylor expansion 328 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 329 | 330 | % Runge-Kutta - 4th order 331 | % h = dt; 332 | k1 = tdynamics(W1); 333 | k2 = tdynamics((W1 + k1*h/2)); 334 | k3 = tdynamics((W1 + k2*h/2)); 335 | k4 = tdynamics((W1 + k3*h)); 336 | 337 | T2 = T1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 338 | 339 | 340 | function w_dot = wdynamics(~, U, J) 341 | w_dot = U/J; 342 | 343 | function t_dot = tdynamics(w) 344 | t_dot = w; 345 | 346 | 347 | function X2 = real_system_dynamics(X1, U, J1, J2, J3, h) 348 | %calculates next stage (k+1) states 349 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 350 | %first order taylor expansion 351 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 352 | 353 | % Runge-Kutta - 4th order 354 | % h = dt; 355 | k1 = spacecraft_dynamics_list( X1 , U, J1, J2, J3); 356 | k2 = spacecraft_dynamics_list((X1 + k1*h/2), U, J1, J2, J3); 357 | k3 = spacecraft_dynamics_list((X1 + k2*h/2), U, J1, J2, J3); 358 | k4 = spacecraft_dynamics_list( (X1 + k3*h), U, J1, J2, J3); 359 | 360 | X2 = X1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 361 | q_sqsum = sqrt(X2(4)^2 + X2(5)^2 + X2(6)^2 + X2(7)^2); 362 | X2(4:7) = X2(4:7)/q_sqsum; 363 | 364 | 365 | function X_dot = spacecraft_dynamics_list(X, U, J1, J2, J3) 366 | %returns the derivatives x_dot = f(X,u) 367 | % FIXED q~ convention 368 | X_dot = zeros(size(X)); 369 | x1 = X(:,1); 370 | x2 = X(:,2); 371 | x3 = X(:,3); 372 | x4 = X(:,4); 373 | x5 = X(:,5); 374 | x6 = X(:,6); 375 | x7 = X(:,7); 376 | u1 = U(:,1); 377 | u2 = U(:,2); 378 | u3 = U(:,3); 379 | %J is assumed to be diagonal, J12 = J23 = ... = 0 380 | X_dot(:,1) = (J2-J3)/J1*x2.*x3 + u1/J1; 381 | X_dot(:,2) = (J3-J1)/J2*x3.*x1 + u2/J2; 382 | X_dot(:,3) = (J1-J2)/J3*x1.*x2 + u3/J3; 383 | X_dot(:,4) = 0.5*(x3.*x5 -x2.*x6 +x1.*x7); 384 | X_dot(:,5) = 0.5*(-x3.*x4 +x1.*x6 +x2.*x7); 385 | X_dot(:,6) = 0.5*(x2.*x4 -x1.*x5 +x3.*x7); 386 | X_dot(:,7) = 0.5*(-x1.*x4 -x2.*x5 -x3.*x6); 387 | -------------------------------------------------------------------------------- /pos-att/Solver_pos_att.m: -------------------------------------------------------------------------------- 1 | classdef Solver_pos_att < handle 2 | %SOLVER_POSITION Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | N % number of stages 7 | Mass % Mass 8 | InertiaM % Moment of Inertia Matrix 9 | J1 % element 1 of Inertia Matrix 10 | J2 % ... 11 | J3 % .. 12 | 13 | size_state_mat 14 | 15 | T_vector % Thruster Forces that are applied as control inputs 16 | 17 | %pos 18 | v_min 19 | v_max 20 | n_mesh_v 21 | 22 | x_min 23 | x_max 24 | n_mesh_x 25 | 26 | %att 27 | w_min 28 | w_max 29 | n_mesh_w 30 | 31 | theta1_min 32 | theta1_max 33 | theta2_min 34 | theta2_max 35 | theta3_min 36 | theta3_max 37 | n_mesh_t 38 | 39 | % 40 | Qx1 41 | Qx2 42 | Qx3 43 | Qv1 44 | Qv2 45 | Qv3 46 | 47 | Qt1 48 | Qt2 49 | Qt3 50 | Qw1 51 | Qw2 52 | Qw3 53 | 54 | R1 55 | R2 56 | R3 57 | 58 | T_final 59 | h 60 | N_stage 61 | defaultX0 62 | 63 | %Thruster and Torque forces 64 | F_Thr0 %Thruster 0 - Force 65 | F_Thr1 66 | F_Thr2 67 | F_Thr3 68 | F_Thr4 69 | F_Thr5 70 | F_Thr6 71 | F_Thr7 72 | F_Thr8 73 | F_Thr9 74 | F_Thr10 75 | F_Thr11 76 | 77 | T_dist %Thruster placement distanceS 78 | % 79 | %Optimum values for each Thruster 80 | Opt_F_Thr0 81 | Opt_F_Thr1 82 | Opt_F_Thr2 83 | Opt_F_Thr3 84 | Opt_F_Thr4 85 | Opt_F_Thr5 86 | Opt_F_Thr6 87 | Opt_F_Thr7 88 | Opt_F_Thr8 89 | Opt_F_Thr9 90 | Opt_F_Thr10 91 | Opt_F_Thr11 92 | 93 | end 94 | 95 | methods 96 | function this = Solver_pos_att() 97 | if nargin < 1 98 | 99 | %pos 100 | this.v_min = -0.1; 101 | this.v_max = +0.1; 102 | this.n_mesh_v = 30; 103 | 104 | this.x_min = -0.2; 105 | this.x_max = 0.2; 106 | this.n_mesh_x = 30; 107 | 108 | %att 109 | this.w_min = deg2rad(-2); 110 | this.w_max = deg2rad(2); 111 | this.n_mesh_w = 15; 112 | 113 | this.theta1_min = -5; %angles of rotation about y-axis (pitch) 114 | this.theta1_max = 5; 115 | this.theta2_min = -6; %angles of rotation about z-axis (yaw) 116 | this.theta2_max = 6; 117 | this.theta3_min = -7; %x-axis rotation 118 | this.theta3_max = 7; 119 | this.n_mesh_t = 20; 120 | % 121 | 122 | this.Mass = 4.16; 123 | inertia(1,1) = 0.02836 + 0.00016; 124 | inertia(2,1) = 0.026817 + 0.00150; 125 | inertia(3,1) = 0.023 + 0.00150; 126 | inertia(4,1) = -0.0000837; 127 | inertia(5,1) = 0.000014; 128 | inertia(6,1) = -0.00029; 129 | this.InertiaM = [inertia(1,1) inertia(4,1) inertia(5,1);... 130 | inertia(4,1) inertia(2,1) inertia(6,1);... 131 | inertia(5,1) inertia(6,1) inertia(3,1)]; 132 | 133 | this.J1 = this.InertiaM(1); 134 | this.J2 = this.InertiaM(5); 135 | this.J3 = this.InertiaM(9); 136 | 137 | this.Qx1 = 6; 138 | this.Qx2 = 6; 139 | this.Qx3 = 6; 140 | this.Qv1 = 6; 141 | this.Qv2 = 6; 142 | this.Qv3 = 6; 143 | 144 | this.Qt1 = .5; 145 | this.Qt2 = .5; 146 | this.Qt3 = .5; 147 | this.Qw1 = .5; 148 | this.Qw2 = .5; 149 | this.Qw3 = .5; 150 | 151 | this.R1 = 0.1; 152 | this.R2 = 0.1; 153 | this.R3 = 0.1; 154 | 155 | this.T_final = 10; 156 | this.h = 0.005; 157 | end 158 | 159 | this.N_stage = this.T_final/this.h; 160 | 161 | if(~isinteger( this.N_stage)) 162 | this.N_stage = ceil(this.N_stage); 163 | this.T_final = this.h*this.N_stage; 164 | warning('T_final is not a factor of h (dt), increasing T_final to %.2f\n',this.T_final) 165 | end 166 | 167 | q0 = []; 168 | this.defaultX0 = [0;0;0;0;0;0;q0;0;0;0]; % [x;v;q;w] 169 | 170 | %Thruster Forces 171 | Thruster_max_F = 0.13; % (N) 172 | this.T_dist = 9.65E-2; % (meters) 173 | %x-direction 174 | this.F_Thr0 = [0 Thruster_max_F]; 175 | this.F_Thr1 = [0 Thruster_max_F]; 176 | 177 | this.F_Thr6 = -[0 Thruster_max_F]; 178 | this.F_Thr7 = -[0 Thruster_max_F]; 179 | 180 | %y-direction 181 | this.F_Thr2 = [0 Thruster_max_F]; 182 | this.F_Thr3 = [0 Thruster_max_F]; 183 | 184 | this.F_Thr8 = -[0 Thruster_max_F]; 185 | this.F_Thr9 = -[0 Thruster_max_F]; 186 | 187 | %z-direction 188 | this.F_Thr4 = [0 Thruster_max_F]; 189 | this.F_Thr5 = [0 Thruster_max_F]; 190 | 191 | this.F_Thr10 = -[0 Thruster_max_F]; 192 | this.F_Thr11 = -[0 Thruster_max_F]; 193 | 194 | 195 | end 196 | 197 | function simplified_run(obj) 198 | 199 | %% mesh generation 200 | s_x1 = sym_linspace(obj, obj.x_min, obj.x_max, obj.n_mesh_x); 201 | s_x2 = sym_linspace(obj, obj.x_min, obj.x_max, obj.n_mesh_x); 202 | s_x3 = sym_linspace(obj, obj.x_min, obj.x_max, obj.n_mesh_x); 203 | 204 | s_v1 = sym_linspace(obj, obj.v_min, obj.v_max, obj.n_mesh_v); 205 | s_v2 = sym_linspace(obj, obj.v_min, obj.v_max, obj.n_mesh_v); 206 | s_v3 = sym_linspace(obj, obj.v_min, obj.v_max, obj.n_mesh_v); 207 | 208 | s_t1 = sym_linspace(obj, deg2rad(obj.theta1_min), deg2rad(obj.theta1_max), obj.n_mesh_t); 209 | s_t2 = sym_linspace(obj, deg2rad(obj.theta2_min), deg2rad(obj.theta2_max), obj.n_mesh_t); 210 | s_t3 = sym_linspace(obj, deg2rad(obj.theta3_min), deg2rad(obj.theta3_max), obj.n_mesh_t); 211 | 212 | s_w1 = sym_linspace(obj, obj.w_min, obj.w_max, obj.n_mesh_w); 213 | s_w2 = sym_linspace(obj, obj.w_min, obj.w_max, obj.n_mesh_w); 214 | s_w3 = sym_linspace(obj, obj.w_min, obj.w_max, obj.n_mesh_w); 215 | 216 | 217 | calculate_one_channel_U_Opt(obj, s_x1,s_v1,s_t1,s_w1, ... 218 | obj.F_Thr0,obj.F_Thr1,obj.F_Thr6,obj.F_Thr7,... 219 | obj.Qx1,obj.Qv1,obj.Qt1,obj.Qw1,obj.R1,... 220 | obj.J2,... 221 | 'channel_x_controller_1'); 222 | 223 | calculate_one_channel_U_Opt(obj, s_x2,s_v2,s_t2,s_w2, ... 224 | obj.F_Thr2,obj.F_Thr3,obj.F_Thr8,obj.F_Thr9,... 225 | obj.Qx2,obj.Qv2,obj.Qt2,obj.Qw2,obj.R2,... 226 | obj.J3,... 227 | 'channel_y_controller_1'); 228 | 229 | calculate_one_channel_U_Opt(obj, s_x3,s_v3,s_t3,s_w3, ... 230 | obj.F_Thr4,obj.F_Thr5,obj.F_Thr10,obj.F_Thr11,... 231 | obj.Qx3,obj.Qv3,obj.Qt3,obj.Qw3,obj.R3,... 232 | obj.J1,... 233 | 'channel_z_controller_1'); 234 | 235 | %calculate failure mode in x direction 236 | calculate_one_channel_U_Opt(obj, s_x1,s_v1,s_t1,s_w1, ... 237 | [0] ,obj.F_Thr1,obj.F_Thr6,obj.F_Thr7,... 238 | obj.Qx1,obj.Qv1,obj.Qt1,obj.Qw1,obj.R1,... 239 | obj.J2,... 240 | 'channel_x_controller_1_failure'); 241 | 242 | end 243 | 244 | function calculate_one_channel_U_Opt(obj, s_x,s_v,s_t,s_w, ... 245 | f0, f1, f6, f7,Qx,Qv,Qt,Qw,R,J,file_name) 246 | 247 | n_x = length(s_x); 248 | n_v = length(s_v); 249 | n_t = length(s_t); 250 | n_w = length(s_w); 251 | 252 | %% initialization 253 | [f0_allcomb,f1_allcomb,f6_allcomb,f7_allcomb] = ... 254 | vectors_allcomb(obj, f0, f1, f6, f7); 255 | %calculating next stage states 256 | %% J input to the next states must be in accordance with the Moment direction 257 | [x_next,v_next,t_next,w_next] = next_stage_states_simplified(obj, s_x,s_v,s_t,s_w,... 258 | f0_allcomb, f1_allcomb, f6_allcomb, f7_allcomb, J); 259 | 260 | %calculating J fixed 261 | J_current = J_current_reshaped(obj, s_x,s_v,s_t,s_w,... 262 | f0_allcomb, f1_allcomb, f6_allcomb, f7_allcomb,... 263 | Qx,Qv,Qt,Qw,R); 264 | F_gI = griddedInterpolant({s_x,s_v,s_t,s_w},... 265 | zeros(n_x,n_v,n_t,n_w,'single'),'linear'); 266 | 267 | 268 | fsum50_prev = 0; 269 | tol = 1e-2; 270 | for k_s = obj.N_stage-1:-1:1 271 | tic 272 | [F_gI.Values, U_Optimal_id] = min( J_current + F_gI(x_next,v_next,t_next,w_next), [], 5); 273 | if ~rem(k_s,50) 274 | fsum50 = sum(F_gI.Values(:)); 275 | idsum50 = sum(U_Optimal_id(:)); 276 | e = fsum50 - fsum50_prev; 277 | e2 = idsum50 - idsum50_prev; 278 | fprintf('stage %d - %f seconds - errorF %f - errorU %f\n', k_s, toc, e, e2) 279 | fsum50_prev = fsum50; 280 | idsum50_prev = idsum50; 281 | if(abs(e) < tol) 282 | fprintf('sum of errors in the last 50 stages is under tolerance, breaking loop...\n') 283 | break 284 | end 285 | end 286 | end 287 | 288 | clear('x_next','v_next','t_next','w_next','J_current') 289 | save(file_name,'F_gI','U_Optimal_id','f0_allcomb','f1_allcomb','f6_allcomb','f7_allcomb') 290 | % obj.Opt_F_Thr0 = f0_allcomb(U_Optimal_id); 291 | % obj.Opt_F_Thr1 = f1_allcomb(U_Optimal_id); 292 | % obj.Opt_F_Thr6 = f6_allcomb(U_Optimal_id); 293 | % obj.Opt_F_Thr7 = f7_allcomb(U_Optimal_id); 294 | %finish up 295 | fprintf('\nstage calculations complete.\n') 296 | 297 | end 298 | 299 | function [x_next,v_next,t_next,w_next] = next_stage_states_simplified(obj, X, V, T, W, f1,f2,f6,f7 ,J) 300 | %store length 301 | L_x = length(X); 302 | L_v = length(V); 303 | L_t = length(T); 304 | L_w = length(W); 305 | 306 | % reshape 307 | X = reshape(X,[L_x 1] ); 308 | V = reshape(V,[1 L_v] ); 309 | T = reshape(T,[1 1 L_t] ); 310 | W = reshape(W,[1 1 1 L_w] ); 311 | f1 = reshape(f1,[1 1 1 1 length(f1)] ); 312 | f2 = reshape(f2,[1 1 1 1 length(f2)] ); 313 | f6 = reshape(f6,[1 1 1 1 length(f6)] ); 314 | f7 = reshape(f7,[1 1 1 1 length(f7)] ); 315 | 316 | % ODE solve 317 | x_next = RK4_x(obj, X, V); 318 | v_next = RK4_v(obj, V, f1,f2,f6,f7); 319 | t_next = RK4_t(obj, T, W); 320 | w_next = RK4_w(obj, W, f1,f2,f6,f7 , J); 321 | 322 | 323 | %repmat each matrix to full size, as required for F inputs 324 | x_next = repmat(x_next,[1 1 L_t L_w length(f1)]); 325 | v_next = repmat(v_next,[L_x 1 L_t L_w 1]); 326 | t_next = repmat(t_next,[L_x L_v 1 1 length(f1)]); 327 | w_next = repmat(w_next,[L_x L_v L_t 1 1]); 328 | end 329 | 330 | function X2 = RK4_x(obj, X1, V) 331 | % Runge-Kutta - 4th order 332 | % h = dt; 333 | k1 = xdynamics(obj, V); 334 | % k2 = xdynamics(obj,(V + k1*obj.h/2)); 335 | % k3 = xdynamics(obj,(V + k2*obj.h/2)); 336 | % k4 = xdynamics(obj,(V + k3*obj.h)); 337 | 338 | X2 = X1 + obj.h*k1; 339 | 340 | end 341 | 342 | function x_dot = xdynamics(~,v) 343 | x_dot = v; 344 | end 345 | 346 | function V2 = RK4_v(obj, V1, f1,f2,f6,f7) % does not need RK4, ki's are equal 347 | % Runge-Kutta - 4th order 348 | % h = dt; 349 | k1 = vdynamics(obj, V1 , f1,f2,f6,f7); 350 | % k2 = vdynamics(obj,(V1 + k1*obj.h/2), f1,f2,f6,f7); 351 | % k3 = vdynamics(obj,(V1 + k2*obj.h/2), f1,f2,f6,f7); 352 | % k4 = vdynamics(obj,(V1 + k3*obj.h), f1,f2,f6,f7); 353 | 354 | V2 = V1 + obj.h*k1; 355 | end 356 | 357 | function v_dot = vdynamics(obj, ~, f1,f2,f6,f7) 358 | v_dot = (f1+f2+f6+f7)/obj.Mass; 359 | end 360 | 361 | function T2 = RK4_t(obj, T1, W1) 362 | %calculates next stage (k+1) states 363 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 364 | %first order taylor expansion 365 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 366 | 367 | % Runge-Kutta - 4th order 368 | % h = dt; 369 | k1 = tdynamics(obj,W1); 370 | % k2 = tdynamics(obj,(W1 + k1*obj.h/2)); 371 | % k3 = tdynamics(obj,(W1 + k2*obj.h/2)); 372 | % k4 = tdynamics(obj,(W1 + k3*obj.h)); 373 | 374 | T2 = T1 + obj.h*k1; 375 | end 376 | 377 | 378 | function t_dot = tdynamics(~,w) 379 | t_dot = w; 380 | end 381 | 382 | function W2 = RK4_w(obj, w, f1,f2,f6,f7, J) 383 | %calculates next stage (k+1) states 384 | % X2 = X1 + dt*a_d(X,u) where a_d is the spacecraft dynamics 385 | %first order taylor expansion 386 | %X2 = X1 + dt*spacecraft_dynamics(spacecraft, X1, U); 387 | 388 | % Runge-Kutta - 4th order 389 | % h = dt; 390 | k1 = wdynamics(obj,w , f1,f2,f6,f7, J); 391 | % k2 = wdynamics(obj,(w + k1*obj.h/2), f1,f2,f6,f7, J); 392 | % k3 = wdynamics(obj,(w + k2*obj.h/2), f1,f2,f6,f7, J); 393 | % k4 = wdynamics(obj,(w + k3*obj.h), f1,f2,f6,f7, J); 394 | 395 | W2 = w + obj.h*(k1); 396 | end 397 | 398 | 399 | function w_dot = wdynamics(obj,~, f1,f2,f6,f7, J) 400 | w_dot = (f1*obj.T_dist +f2*(-obj.T_dist) + ... 401 | f6*obj.T_dist +f7*(-obj.T_dist) )/J; 402 | end 403 | 404 | function [f0,f1,f2,f3,f4,f5,f6,f7,f8,f9,f10,f11] = get_thruster_on_off_optimal(obj,x,v,t,w,R0,V0,q) 405 | % gets the optimal on/off state of thrusters, method run() or 406 | % simplified_run() must be run before calling this function, 407 | % the inputs x,v are position and velocity of object B 408 | % to target A in RSW frame 409 | 410 | %transform vectors to body frame of reference 411 | rotM_RSW2ECI = RSW2ECI(obj, R0, V0); 412 | rotM_ECI2body = ECI2body(obj,q); 413 | 414 | x = rotM_ECI2body*(rotM_RSW2ECI* x'); 415 | v = rotM_ECI2body*(rotM_RSW2ECI* v'); 416 | 417 | % 418 | t_x = t(1); %rotation about x_axis 419 | t_y = t(2); 420 | t_z = t(3); 421 | 422 | w_x = w(1); %rotational speed about x_axis 423 | w_y = w(2); 424 | w_z = w(3); 425 | 426 | x1 = x(1); 427 | x2 = x(2); 428 | x3 = x(3); 429 | 430 | v1 = v(1); 431 | v2 = v(2); 432 | v3 = v(3); 433 | 434 | f0 = obj.Opt_F_Thr0(x1,v1, t_y, w_y ); 435 | f1 = obj.Opt_F_Thr1(x1,v1, t_y, w_y ); 436 | f6 = obj.Opt_F_Thr6(x1,v1, t_y, w_y ); 437 | f7 = obj.Opt_F_Thr7(x1,v1, t_y, w_y ); 438 | 439 | f2 = obj.Opt_F_Thr2(x2,v2, t_z, w_z ); 440 | f3 = obj.Opt_F_Thr3(x2,v2, t_z, w_z ); 441 | f8 = obj.Opt_F_Thr8(x2,v2, t_z, w_z ); 442 | f9 = obj.Opt_F_Thr9(x2,v2, t_z, w_z ); 443 | 444 | f4 = obj.Opt_F_Thr4(x3,v3, t_x, w_x ); 445 | f5 = obj.Opt_F_Thr5(x3,v3, t_x, w_x ); 446 | f10 = obj.Opt_F_Thr10(x3,v3, t_x, w_x ); 447 | f11 = obj.Opt_F_Thr11(x3,v3, t_x, w_x ); 448 | 449 | end 450 | 451 | 452 | function get_optimal_path(obj) 453 | global mu 454 | mu = 398600; 455 | if nargin < 2 456 | % X0 = obj.defaultX0; 457 | % Prescribed initial state vector of chaser B in the co-moving frame: 458 | dr0 = [-0.1 0 0]; 459 | dv0 = [0 0 0]; 460 | 461 | % q0 = [0 0 0 1]; 462 | q0 = angle2quat(deg2rad(0),deg2rad(3),deg2rad(0)); 463 | q0 = q0(end:-1:1); 464 | 465 | w0 = [0 0 0]; 466 | X0 = [dr0 dv0 q0 w0]'; 467 | tf = obj.T_final; 468 | N_total_sim = obj.N_stage; 469 | end 470 | %% set controllers 471 | obj.set_controller('channel_x_controller_1.mat','x') 472 | obj.set_controller('channel_y_controller_1.mat','y') 473 | obj.set_controller('channel_z_controller_1.mat','z') 474 | 475 | %% 476 | tspan = 0:obj.h:tf; 477 | X_ode45 = zeros(N_total_sim, 13); 478 | F_Th_Opt = zeros(N_total_sim, 12); 479 | Force_Moment_log = zeros(N_total_sim, 6); 480 | X_ode45(1,:) = X0; 481 | % Calculate the target initial state vector 482 | [R0,V0] = get_target_R0V0(obj); 483 | tic 484 | for k_stage=1:N_total_sim-1 485 | %determine F_Opt each Thruster 486 | X_stage = X_ode45(k_stage,:); 487 | 488 | x_stage = X_stage(1:3); 489 | v_stage = X_stage(4:6); 490 | t_stage = [2*asin(X_stage(7));... %angle about x-axis 491 | 2*asin(X_stage(8));... %y-axis 492 | 2*asin(X_stage(9))]; %z-axis 493 | w_stage = X_stage(11:13); 494 | q_stage = X_stage(7:10); 495 | [f0,f1,f2,f3,f4,f5,f6,f7,f8,f9,f10,f11] = ... 496 | get_thruster_on_off_optimal(obj, x_stage, v_stage, t_stage, w_stage, R0,V0,q_stage); 497 | 498 | % calculate moments (U_M) and directional forces (a_* |x,y,z|) 499 | [U_M, a_x, a_y, a_z] = to_Moments_Forces(obj,f0,f1,f2,f3,f4,f5,f6,f7,f8,f9,f10,f11,R0,V0,q_stage); 500 | %log 501 | F_Th_Opt(k_stage,:) = [f0,f1,f2,f3,f4,f5,f6,f7,f8,f9,f10,f11]; 502 | Force_Moment_log(k_stage,:) = [a_x, a_y, a_z, U_M']; 503 | % 504 | [~,X_temp] = ode45(@ode_eq,[tspan(k_stage), tspan(k_stage+1)], X_stage); 505 | X_ode45(k_stage+1,:) = X_temp(end,:); 506 | end 507 | toc 508 | T_ode45 = tspan(1:end-1)'; 509 | %plot Thruster Firings 510 | ylim_thr = [-.15 .15]; 511 | pos_fig_thruster = [7.4000 61.8000 538.4000 712.8000]; 512 | figure('Name','Thruster Firings','Position',pos_fig_thruster) 513 | subplot(4,3,1) 514 | plot(T_ode45, F_Th_Opt(:,1)) 515 | title('#0 (x)') 516 | grid on 517 | ylim(ylim_thr) 518 | %% xlim([-0.05 Inf]) 519 | 520 | subplot(4,3,2) 521 | plot(T_ode45, F_Th_Opt(:,3)) 522 | title('#2 (y)') 523 | grid on 524 | ylim(ylim_thr) 525 | 526 | subplot(4,3,3) 527 | plot(T_ode45, F_Th_Opt(:,5)) 528 | title('#4 (z)') 529 | grid on 530 | ylim(ylim_thr) 531 | 532 | subplot(4,3,4) 533 | plot(T_ode45, F_Th_Opt(:,2)) 534 | title('#1 (x)') 535 | grid on 536 | ylim(ylim_thr) 537 | 538 | subplot(4,3,5) 539 | plot(T_ode45, F_Th_Opt(:,4)) 540 | title('#3 (y)') 541 | grid on 542 | ylim(ylim_thr) 543 | 544 | subplot(4,3,6) 545 | plot(T_ode45, F_Th_Opt(:,6)) 546 | title('#5 (z)') 547 | grid on 548 | ylim(ylim_thr) 549 | 550 | subplot(4,3,7) 551 | plot(T_ode45, F_Th_Opt(:,7)) 552 | title('#6 (-x)') 553 | grid on 554 | ylim(ylim_thr) 555 | 556 | subplot(4,3,8) 557 | plot(T_ode45, F_Th_Opt(:,9)) 558 | title('#8 (-y)') 559 | grid on 560 | ylim(ylim_thr) 561 | 562 | subplot(4,3,9) 563 | plot(T_ode45, F_Th_Opt(:,11)) 564 | title('#10 (-z)') 565 | grid on 566 | ylim(ylim_thr) 567 | 568 | subplot(4,3,10) 569 | plot(T_ode45, F_Th_Opt(:,8)) 570 | title('#7 (-x)') 571 | grid on 572 | ylim(ylim_thr) 573 | 574 | subplot(4,3,11) 575 | plot(T_ode45, F_Th_Opt(:,10)) 576 | title('#9 (-y)') 577 | grid on 578 | ylim(ylim_thr) 579 | 580 | subplot(4,3,12) 581 | plot(T_ode45, F_Th_Opt(:,12)) 582 | title('#11 (-z)') 583 | grid on 584 | ylim(ylim_thr) 585 | 586 | %plot Moments 587 | ylim_F = [-0.3 0.3]/obj.Mass; 588 | ylim_M = [-0.3 0.3]*obj.T_dist; 589 | pos_fig_FM = [547.4000 449.8000 518.4000 326.4000]; 590 | figure('Name','Forces and Moments','Position',pos_fig_FM) 591 | 592 | subplot(3,2,1) 593 | plot(T_ode45, Force_Moment_log(:,1)) 594 | title('acceleration x-direction') 595 | grid on 596 | ylim(ylim_F) 597 | 598 | subplot(3,2,3) 599 | plot(T_ode45, Force_Moment_log(:,2)) 600 | title('acceleration y-direction') 601 | grid on 602 | ylim(ylim_F) 603 | 604 | subplot(3,2,5) 605 | plot(T_ode45, Force_Moment_log(:,3)) 606 | title('acceleration z-direction') 607 | grid on 608 | ylim(ylim_F) 609 | 610 | subplot(3,2,2) 611 | plot(T_ode45, Force_Moment_log(:,4)) 612 | title('Moment x-direction') 613 | grid on 614 | ylim(ylim_M) 615 | 616 | subplot(3,2,4) 617 | plot(T_ode45, Force_Moment_log(:,5)) 618 | title('Moment y-direction') 619 | grid on 620 | ylim(ylim_M) 621 | 622 | subplot(3,2,6) 623 | plot(T_ode45, Force_Moment_log(:,6)) 624 | title('Moment z-direction') 625 | grid on 626 | ylim(ylim_M) 627 | 628 | % plot states - pos 629 | pos_fig_x = [543.4000 49.0000 518.4000 326.4000]; 630 | figure('Name','states - position','Position',pos_fig_x) 631 | 632 | plot(T_ode45, X_ode45(:,1)) 633 | hold on 634 | plot(T_ode45, X_ode45(:,2)) 635 | plot(T_ode45, X_ode45(:,3)) 636 | grid on 637 | legend('x1','x2','x3') 638 | 639 | % plot states - v 640 | pos_fig_v = [954.6000 446.6000 518.4000 326.4000]; 641 | figure('Name','states - velocity','Position',pos_fig_v) 642 | 643 | plot(T_ode45, X_ode45(:,4)) 644 | hold on 645 | plot(T_ode45, X_ode45(:,5)) 646 | plot(T_ode45, X_ode45(:,6)) 647 | grid on 648 | legend('v1','v2','v3') 649 | 650 | % plot states - q 651 | pos_fig_q = [973.0000 218.6000 518.4000 326.4000]; 652 | figure('Name','states - quaternions','Position',pos_fig_q) 653 | 654 | plot(T_ode45, X_ode45(:,7)) 655 | hold on 656 | plot(T_ode45, X_ode45(:,8)) 657 | plot(T_ode45, X_ode45(:,9)) 658 | plot(T_ode45, X_ode45(:,10)) 659 | grid on 660 | legend('q1','q2','q3','q4') 661 | 662 | % plot states - w 663 | pos_fig_w = [956.2000 47.4000 518.4000 326.4000]; 664 | figure('Name','states - rotational speeds','Position',pos_fig_w) 665 | 666 | title('states - rotational speeds') 667 | plot(T_ode45, X_ode45(:,11)) 668 | hold on 669 | plot(T_ode45, X_ode45(:,12)) 670 | plot(T_ode45, X_ode45(:,13)) 671 | grid on 672 | legend('w1','w2','w3') 673 | 674 | %function declarations 675 | function x_dot = ode_eq(t,X1) 676 | x_dot = system_dynamics(t,X1); 677 | x_dot = x_dot'; 678 | function X_dot = system_dynamics(t,X) 679 | x1 = X(1); 680 | x2 = X(2); 681 | x3 = X(3); 682 | v1 = X(4); 683 | v2 = X(5); 684 | v3 = X(6); 685 | q1 = X(7); 686 | q2 = X(8); 687 | q3 = X(9); 688 | q4 = X(10); 689 | w1 = X(11); 690 | w2 = X(12); 691 | w3 = X(13); 692 | w_vector = X(11:13); 693 | %--- differential equations ------------------------- 694 | % pre-computations 695 | [R,V] = update_RV_target(obj, R0, V0, t); 696 | norm_R = (R*R')^.5; %norm R 697 | RdotV = sum(R.*V); %dot product 698 | crossRV = [R(2).*V(3)-R(3).*V(2); % cross product of R and V 699 | R(3).*V(1)-R(1).*V(3); 700 | R(1).*V(2)-R(2).*V(1)]; 701 | H = (crossRV'*crossRV)^.5 ; %norm(crossRV); 702 | 703 | % CW-equations 704 | % position - x 705 | X_dot(1) = v1; 706 | X_dot(2) = v2; 707 | X_dot(3) = v3; 708 | 709 | % position - v (a_x,y,z are in RSW frame of reference) 710 | X_dot(4) = (2*mu/norm_R^3 + H^2/norm_R^4)*x1 - 2*RdotV/norm_R^4*H*x2 + 2*H/norm_R^2*v2 ... 711 | + a_x; 712 | X_dot(5) = -(mu/norm_R^3 - H^2/norm_R^4)*x2 + 2*RdotV/norm_R^4*H*x1 - 2*H/norm_R^2*v1 ... 713 | + a_y; 714 | X_dot(6) = -mu/norm_R^3*x3 ... 715 | + a_z; 716 | 717 | % attitude - q 718 | X_dot(7) = 0.5*(w3.*q2 -w2.*q3 +w1.*q4); 719 | X_dot(8) = 0.5*(-w3.*q1 +w1.*q3 +w2.*q4); 720 | X_dot(9) = 0.5*(w2.*q1 -w1.*q2 +w3.*q4); 721 | X_dot(10) = 0.5*(-w1.*q1 -w2.*q2 -w3.*q3); 722 | 723 | % attitude - w 724 | w_dot = obj.InertiaM\(U_M - cross(w_vector, obj.InertiaM*w_vector)); 725 | X_dot(11) = w_dot(1); 726 | X_dot(12) = w_dot(2); 727 | X_dot(13) = w_dot(3); 728 | end 729 | end 730 | end 731 | 732 | 733 | 734 | function [R0,V0] = get_target_R0V0(obj) 735 | global mu 736 | RE = 6378; 737 | %...Input data: 738 | % Prescribed initial orbital parameters of target A: 739 | rp = RE + 300; 740 | e = 0.1; 741 | i = 0; 742 | RA = 0; 743 | omega = 0; 744 | theta = 0; 745 | % Additional computed parameters: 746 | ra = rp*(1 + e)/(1 - e); 747 | h_ = sqrt(2*mu*rp*ra/(ra + rp)); 748 | a = (rp + ra)/2; 749 | T = 2*pi/sqrt(mu)*a^1.5; 750 | n = 2*pi/T; 751 | [R0,V0] = sv_from_coe([h_ e RA i omega theta],mu); 752 | end 753 | 754 | function [R2,V2] = update_RV_target(~,R0,V0,t) 755 | %Updates the state vectors of the target sat 756 | % mu - gravitational parameter (km^3/s^2) 757 | % R0 - initial position vector (km) 758 | % V0 - initial velocity vector (km/s) 759 | % t - elapsed time (s) 760 | % R - final position vector (km) 761 | % V - final velocity vector (km/s) 762 | global mu 763 | %...Magnitudes of R0 and V0: 764 | r0 = norm(R0); 765 | v0 = norm(V0); 766 | %...Initial radial velocity: 767 | vr0 = dot(R0, V0)/r0; 768 | %...Reciprocal of the semimajor axis (from the energy equation): 769 | alpha = 2/r0 - v0^2/mu; 770 | %...Compute the universal anomaly: 771 | x = kepler_U(t, r0, vr0, alpha); 772 | %...Compute the f and g functions: 773 | [f, g] = f_and_g(x, t, r0, alpha); 774 | %...Compute the final position vector: 775 | R2 = f*R0 + g*V0; 776 | %...Compute the magnitude of R: 777 | r2 = norm(R2); 778 | %...Compute the derivatives of f and g: 779 | [fdot, gdot] = fDot_and_gDot(x, r2, r0, alpha); 780 | %...Compute the final velocity: 781 | V2 = fdot*R0 + gdot*V0; 782 | end 783 | 784 | function J_current_M = J_current_reshaped(~,x,v,t,w,f1,f2,f3,f4,Qx,Qv,Qt,Qw,R) 785 | 786 | L_x = length(x); 787 | L_v = length(v); 788 | L_t = length(t); 789 | L_w = length(w); 790 | 791 | x = reshape(x,[L_x 1] ); 792 | v = reshape(v,[1 L_v] ); 793 | t = reshape(t,[1 1 L_t] ); 794 | w = reshape(w,[1 1 1 L_w] ); 795 | f1 = reshape(f1,[1 1 1 1 length(f1)] ); 796 | f2 = reshape(f2,[1 1 1 1 length(f2)] ); 797 | f3 = reshape(f3,[1 1 1 1 length(f3)] ); 798 | f4 = reshape(f4,[1 1 1 1 length(f4)] ); 799 | % 800 | J_current_M = single(Qx * x.^2 + Qv * v.^2 + Qw * w.^2 + Qt * t.^2 +... 801 | ( R* f1.^2 + R* f2.^2 + R* f3.^2 + R* f4.^2) ); 802 | end 803 | 804 | function [U_M, a_x, a_y, a_z] = to_Moments_Forces(obj,f0,f1,f2,f3,f4,f5,f6,f7,f8,f9,f10,f11,R0,V0,q) 805 | % Moments 806 | U_M_y = (f0-f1+f6-f7)*obj.T_dist; 807 | U_M_z = (f2-f3+f8-f9)*obj.T_dist; 808 | U_M_x = (f4-f5+f10-f11)*obj.T_dist; 809 | U_M = [U_M_x; U_M_y; U_M_z]; 810 | 811 | % Forces (expressed in body frame of reference) 812 | a_x_body = (f0+f1+f6+f7)/obj.Mass; 813 | a_y_body = (f2+f3+f8+f9)/obj.Mass; 814 | a_z_body = (f4+f5+f10+f11)/obj.Mass; 815 | % transform vectors 816 | rotM_RSW2ECI = RSW2ECI(obj, R0, V0); 817 | rotM_ECI2body = ECI2body(obj,q); 818 | 819 | accM = rotM_RSW2ECI\(rotM_ECI2body\[a_x_body a_y_body a_z_body]'); 820 | a_x = accM(1); 821 | a_y = accM(2); 822 | a_z = accM(3); 823 | end 824 | 825 | function qrotMat = ECI2body(~, q) 826 | qrotMat = [1-2*(q(2)^2+q(3)^2), 2*(q(1)*q(2)+q(3)*q(4)), 2*(q(1)*q(3)-q(2)*q(4));... 827 | 2*(q(2)*q(1)-q(3)*q(4)), 1-2*(q(1)^2+q(3)^2), 2*(q(2)*q(3) +q(1)*q(4));... 828 | 2*(q(3)*q(1)+q(2)*q(4)), 2*(q(3)*q(2)-q(1)*q(4)), 1-2*(q(1)^2+q(2)^2)]; 829 | end 830 | 831 | function rotMat = RSW2ECI(~, pos, vel) 832 | % rotMat = RSW2ECI(pos, vel); 833 | % Creates a rotation matrix which transforms RSW vectors to ECI vectors. 834 | % ECIvec = rotMat * RSW; 835 | % 3x1 = 3x3 3x1; 836 | % Inputs: 837 | % pos: ECI position vector 838 | % vel: ECI velocity vector 839 | % Outpus: 840 | % rotMat: 3x3 rotation matrix from RSW to ECI 841 | 842 | R = pos/norm(pos); 843 | W = cross(pos,vel)/norm(cross(pos,vel)); 844 | S = cross(W,R); 845 | 846 | rotMat = [R' S' W']; 847 | end 848 | 849 | function set_controller(obj, file, channel) 850 | C = load(file); 851 | fgI0 = griddedInterpolant(C.F_gI.GridVectors,... 852 | C.f0_allcomb(C.U_Optimal_id),'nearest'); 853 | 854 | fgI1 = griddedInterpolant(C.F_gI.GridVectors,... 855 | C.f1_allcomb(C.U_Optimal_id),'nearest'); 856 | 857 | fgI6 = griddedInterpolant(C.F_gI.GridVectors,... 858 | C.f6_allcomb(C.U_Optimal_id),'nearest'); 859 | 860 | fgI7 = griddedInterpolant(C.F_gI.GridVectors,... 861 | C.f7_allcomb(C.U_Optimal_id),'nearest'); 862 | 863 | switch channel 864 | case 'x' 865 | obj.Opt_F_Thr0 = fgI0; 866 | obj.Opt_F_Thr1 = fgI1; 867 | obj.Opt_F_Thr6 = fgI6; 868 | obj.Opt_F_Thr7 = fgI7; 869 | case 'y' 870 | obj.Opt_F_Thr2 = fgI0; 871 | obj.Opt_F_Thr3 = fgI1; 872 | obj.Opt_F_Thr8 = fgI6; 873 | obj.Opt_F_Thr9 = fgI7; 874 | case 'z' 875 | obj.Opt_F_Thr4 = fgI0; 876 | obj.Opt_F_Thr5 = fgI1; 877 | obj.Opt_F_Thr10 = fgI6; 878 | obj.Opt_F_Thr11 = fgI7; 879 | 880 | otherwise 881 | error('wrong channel, must be one of x-y-z values') 882 | end 883 | 884 | end 885 | 886 | function [f1_a,f2_a,f3_a,f4_a] = vectors_allcomb(~,f1,f2,f3,f4) 887 | [f1,f2,f3,f4] = ndgrid(f1,f2,f3,f4); 888 | f1 = f1(:); 889 | f2 = f2(:); 890 | f3 = f3(:); 891 | f4 = f4(:); 892 | 893 | idrm1 = find( f1 >0 & f3< 0 ); 894 | idrm2 = find( f2 > 0 & f4 < 0); 895 | 896 | idrm = unique([idrm1,idrm2])'; 897 | id = setdiff(1:length(f1),idrm); 898 | 899 | f1_a = f1(id); 900 | f2_a = f2(id); 901 | f3_a = f3(id); 902 | f4_a = f4(id); 903 | 904 | end 905 | 906 | function v = sym_linspace(~,a,b,n) 907 | if(a>0) 908 | error('minimum states are not negative, use normal linspace') 909 | end 910 | if(mod(n,2) == 0) 911 | v_1 = linspace(a,0,ceil((n)/2)+1); 912 | else 913 | v_1 = linspace(a,0,ceil((n)/2)); 914 | end 915 | v_2 = linspace(0,b,ceil((n)/2)); 916 | v_2 = v_2(2:end); %remove first zero 917 | v = [v_1,v_2]; 918 | end 919 | end 920 | 921 | end 922 | 923 | 924 | -------------------------------------------------------------------------------- /pos-att/private/fDot_and_gDot.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function [fdot, gdot] = fDot_and_gDot(x, r, ro, a) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function calculates the time derivatives of the Lagrange f and g coefficients. mu - the gravitational parameter (km^3/s^2) a - reciprocal of the semimajor axis (1/km) ro - the radial position at time to (km) t - the time elapsed since initial state vector (s) r - the radial position after time t (km) x - the universal anomaly after time t (km^0.5) fdot - time derivative of the Lagrange f coefficient (1/s) gdot - time derivative of the Lagrange g coefficient (dimensionless) User M-functions required: stumpC, stumpS %} % -------------------------------------------------- global mu z = a*x^2; %...Equation 3.69c: fdot = sqrt(mu)/r/ro*(z*stumpS(z) - 1)*x; %...Equation 3.69d: gdot = 1 - x^2/r*stumpC(z); % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /pos-att/private/f_and_g.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function [f, g] = f_and_g(x, t, ro, a) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function calculates the Lagrange f and g coefficients. mu - the gravitational parameter (km^3/s^2) a - reciprocal of the semimajor axis (1/km) ro - the radial position at time to (km) t - the time elapsed since ro (s) x - the universal anomaly after time t (km^0.5) f - the Lagrange f coefficient (dimensionless) g - the Lagrange g coefficient (s) User M-functions required: stumpC, stumpS %} % ---------------------------------------------- global mu z = a*x^2; %...Equation 3.69a: f = 1 - x^2/ro*stumpC(z); %...Equation 3.69b: g = t - 1/sqrt(mu)*x^3*stumpS(z); end % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /pos-att/private/kepler_U.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function x = kepler_U(dt, ro, vro, a) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function uses Newton's method to solve the universal Kepler equation for the universal anomaly. mu - gravitational parameter (km^3/s^2) x - the universal anomaly (km^0.5) dt - time since x = 0 (s) ro - radial position (km) when x = 0 vro - radial velocity (km/s) when x = 0 a - reciprocal of the semimajor axis (1/km) z - auxiliary variable (z = a*x^2) C - value of Stumpff function C(z) S - value of Stumpff function S(z) n - number of iterations for convergence nMax - maximum allowable number of iterations User M-functions required: stumpC, stumpS %} % ---------------------------------------------- global mu %...Set an error tolerance and a limit on the number of iterations: error = 1.e-8; nMax = 1000; %...Starting value for x: x = sqrt(mu)*abs(a)*dt; %...Iterate on Equation 3.65 until until convergence occurs within %...the error tolerance: n = 0; ratio = 1; while abs(ratio) > error && n <= nMax n = n + 1; C = stumpC(a*x^2); S = stumpS(a*x^2); F = ro*vro/sqrt(mu)*x^2*C + (1 - a*ro)*x^3*S + ro*x - sqrt(mu)*dt; dFdx = ro*vro/sqrt(mu)*x*(1 - a*x^2*S) + (1 - a*ro)*x^2*C + ro; ratio = F/dFdx; x = x - ratio; end %...Deliver a value for x, but report that nMax was reached: if n > nMax fprintf('\n **No. iterations of Kepler''s equation = %g', n) fprintf('\n F/dFdx = %g\n', F/dFdx) end % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /pos-att/private/rkf45.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 2 | function [tout, yout] = rkf45(ode_function, tspan, y0, tolerance) 3 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 4 | %{ 5 | This function uses the Runge-Kutta-Fehlberg 4(5) algorithm to 6 | integrate a system of first-order differential equations 7 | dy/dt = f(t,y). 8 | 9 | y - column vector of solutions 10 | f - column vector of the derivatives dy/dt 11 | t - time 12 | a - Fehlberg coefficients for locating the six solution 13 | points (nodes) within each time interval. 14 | b - Fehlberg coupling coefficients for computing the 15 | derivatives at each interior point 16 | c4 - Fehlberg coefficients for the fourth-order solution 17 | c5 - Fehlberg coefficients for the fifth-order solution 18 | tol - allowable truncation error 19 | ode_function - handle for user M-function in which the derivatives f 20 | are computed 21 | tspan - the vector [t0 tf] giving the time interval for the 22 | solution 23 | t0 - initial time 24 | tf - final time 25 | y0 - column vector of initial values of the vector y 26 | tout - column vector of times at which y was evaluated 27 | yout - a matrix, each row of which contains the components of y 28 | evaluated at the correponding time in tout 29 | h - time step 30 | hmin - minimum allowable time step 31 | ti - time at the beginning of a time step 32 | yi - values of y at the beginning of a time step 33 | t_inner - time within a given time step 34 | y_inner - values of y witin a given time step 35 | te - trucation error for each y at a given time step 36 | te_allowed - allowable truncation error 37 | te_max - maximum absolute value of the components of te 38 | ymax - maximum absolute value of the components of y 39 | tol - relative tolerance 40 | delta - fractional change in step size 41 | eps - unit roundoff error (the smallest number for which 42 | 1 + eps > 1) 43 | eps(x) - the smallest number such that x + eps(x) = x 44 | 45 | User M-function required: ode_function 46 | %} 47 | % --------------------------------------------------------------- 48 | 49 | a = [0 1/4 3/8 12/13 1 1/2]; 50 | 51 | b = [ 0 0 0 0 0 52 | 1/4 0 0 0 0 53 | 3/32 9/32 0 0 0 54 | 1932/2197 -7200/2197 7296/2197 0 0 55 | 439/216 -8 3680/513 -845/4104 0 56 | -8/27 2 -3544/2565 1859/4104 -11/40]; 57 | 58 | c4 = [25/216 0 1408/2565 2197/4104 -1/5 0 ]; 59 | c5 = [16/135 0 6656/12825 28561/56430 -9/50 2/55]; 60 | 61 | if nargin < 4 62 | tol = 1.e-8; 63 | else 64 | tol = tolerance; 65 | end 66 | 67 | t0 = tspan(1); 68 | tf = tspan(2); 69 | t = t0; 70 | y = y0; 71 | tout = t; 72 | yout = y'; 73 | h = (tf - t0)/100; % Assumed initial time step. 74 | 75 | while t < tf 76 | hmin = 16*eps(t); 77 | ti = t; 78 | yi = y; 79 | %...Evaluate the time derivative(s) at six points within the current 80 | % interval: 81 | for i = 1:6 82 | t_inner = ti + a(i)*h; 83 | y_inner = yi; 84 | for j = 1:i-1 85 | y_inner = y_inner + h*b(i,j)*f(:,j); 86 | end 87 | f(:,i) = feval(ode_function, t_inner, y_inner); 88 | end 89 | 90 | %...Compute the maximum truncation error: 91 | te = h*f*(c4' - c5'); % Difference between 4th and 92 | % 5th order solutions 93 | te_max = max(abs(te)); 94 | 95 | %...Compute the allowable truncation error: 96 | ymax = max(abs(y)); 97 | te_allowed = tol*max(ymax,1.0); 98 | 99 | %...Compute the fractional change in step size: 100 | delta = (te_allowed/(te_max + eps))^(1/5); 101 | 102 | %...If the truncation error is in bounds, then update the solution: 103 | if te_max <= te_allowed 104 | h = min(h, tf-t); 105 | t = t + h; 106 | y = yi + h*f*c5'; 107 | tout = [tout;t]; 108 | yout = [yout;y']; 109 | end 110 | 111 | %...Update the time step: 112 | h = min(delta*h, 4*h); 113 | if h < hmin 114 | fprintf(['\n\n Warning: Step size fell below its minimum\n'... 115 | ' allowable value (%g) at time %g.\n\n'], hmin, t) 116 | return 117 | end 118 | end 119 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 120 | -------------------------------------------------------------------------------- /pos-att/private/stumpC.m: -------------------------------------------------------------------------------- 1 | function c = stumpC(z) 2 | % wwwwwwwwwwwwwwwwwwwwww 3 | %{ 4 | This function evaluates the Stumpff function C(z) according 5 | to Equation 3.53. 6 | z - input argument 7 | c - value of C(z) 8 | User M-functions required: none 9 | %} 10 | % ---------------------------------------------- 11 | if z > 0 12 | c = (1 - cos(sqrt(z)))/z; 13 | elseif z < 0 14 | c = (cosh(sqrt(-z)) - 1)/(-z); 15 | else 16 | c = 1/2; 17 | end -------------------------------------------------------------------------------- /pos-att/private/stumpS.m: -------------------------------------------------------------------------------- 1 | function s = stumpS(z) 2 | % wwwwwwwwwwwwwwwwwwwwww 3 | %{ 4 | This function evaluates the Stumpff function S(z) according 5 | to Equation 3.52. 6 | z - input argument 7 | s - value of S(z) 8 | User M-functions required: none 9 | %} 10 | % ---------------------------------------------- 11 | if z > 0 12 | s = (sqrt(z) - sin(sqrt(z)))/(sqrt(z))^3; 13 | elseif z < 0 14 | s = (sinh(sqrt(-z)) - sqrt(-z))/(sqrt(-z))^3; 15 | else 16 | s = 1/6; 17 | end 18 | % wwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwww -------------------------------------------------------------------------------- /pos-att/private/sv_from_coe.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function [r, v] = sv_from_coe(coe,mu) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function computes the state vector (r,v) from the classical orbital elements (coe). mu - gravitational parameter (km^3;s^2) coe - orbital elements [h e RA incl w TA] where h = angular momentum (km^2/s) e = eccentricity RA = right ascension of the ascending node (rad) incl = inclination of the orbit (rad) w = argument of perigee (rad) TA = true anomaly (rad) R3_w - Rotation matrix about the z-axis through the angle w R1_i - Rotation matrix about the x-axis through the angle i R3_W - Rotation matrix about the z-axis through the angle RA Q_pX - Matrix of the transformation from perifocal to geocentric equatorial frame rp - position vector in the perifocal frame (km) vp - velocity vector in the perifocal frame (km/s) r - position vector in the geocentric equatorial frame (km) v - velocity vector in the geocentric equatorial frame (km/s) User M-functions required: none %} % ---------------------------------------------- h = coe(1); e = coe(2); RA = coe(3); incl = coe(4); w = coe(5); TA = coe(6); %...Equations 4.45 and 4.46 (rp and vp are column vectors): rp = (h^2/mu) * (1/(1 + e*cos(TA))) * (cos(TA)*[1;0;0] + sin(TA)*[0;1;0]); vp = (mu/h) * (-sin(TA)*[1;0;0] + (e + cos(TA))*[0;1;0]); %...Equation 4.34: R3_W = [ cos(RA) sin(RA) 0 -sin(RA) cos(RA) 0 0 0 1]; %...Equation 4.32: R1_i = [1 0 0 0 cos(incl) sin(incl) 0 -sin(incl) cos(incl)]; %...Equation 4.34: R3_w = [ cos(w) sin(w) 0 -sin(w) cos(w) 0 0 0 1]; %...Equation 4.49: Q_pX = (R3_w*R1_i*R3_W)'; %...Equations 4.51 (r and v are column vectors): r = Q_pX*rp; v = Q_pX*vp; %...Convert r and v into row vectors: r = r'; v = v'; end % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /position-control/Solver_position.m: -------------------------------------------------------------------------------- 1 | classdef Solver_position < handle 2 | %SOLVER_POSITION Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | N % number of stages 7 | Mass % Mass 8 | 9 | size_state_mat 10 | 11 | U_vector % values that are applied as control inputs 12 | % big data 13 | %J_next_states_opt single % optimum values of J will be stored in this matrix and recalled each stage 14 | J_current_state_fix single % values of cost to reach next stage, always fixed in Time Invariant system 15 | 16 | v_min 17 | v_max 18 | 19 | x_min 20 | x_max 21 | n_mesh_v 22 | n_mesh_x 23 | Qx1 24 | Qx2 25 | Qx3 26 | Qv1 27 | Qv2 28 | Qv3 29 | 30 | R1 31 | R2 32 | R3 33 | 34 | T_final 35 | h 36 | N_stage 37 | defaultX0 38 | 39 | U1_Opt 40 | U2_Opt 41 | U3_Opt 42 | 43 | end 44 | 45 | methods 46 | function this = Solver_position() 47 | if nargin < 1 48 | 49 | this.v_min = -0.5; 50 | this.v_max = +0.5; 51 | 52 | this.x_min = -.5; 53 | this.x_max = .5; 54 | 55 | this.n_mesh_v = 200; 56 | this.n_mesh_x = 200; 57 | 58 | this.Mass = 4.16; 59 | 60 | this.Qx1 = 6; 61 | this.Qx2 = 6; 62 | this.Qx3 = 6; 63 | this.Qv1 = 6; 64 | this.Qv2 = 6; 65 | this.Qv3 = 6; 66 | 67 | this.R1 = 0.1; 68 | this.R2 = 0.1; 69 | this.R3 = 0.1; 70 | 71 | this.T_final = 30; 72 | this.h = 0.005; 73 | end 74 | 75 | this.N_stage = this.T_final/this.h; 76 | 77 | if(~isinteger( this.N_stage)) 78 | this.N_stage = ceil(this.N_stage); 79 | this.T_final = this.h*this.N_stage; 80 | warning('T_final is not a factor of h (dt), increasing T_final to %.2f\n',this.T_final) 81 | end 82 | 83 | this.defaultX0 = [0;0;0;0;0;0]; % [x;v] 84 | this.U_vector = [-0.13 0 0.13]*2; % Thruster Force 85 | 86 | %u_opt 87 | size_Umat = [this.n_mesh_x, this.n_mesh_v]; 88 | this.U1_Opt = zeros(size_Umat,'single'); 89 | this.U2_Opt = zeros(size_Umat,'single'); 90 | this.U3_Opt = zeros(size_Umat,'single'); 91 | 92 | end 93 | 94 | function simplified_run(obj) 95 | 96 | %% mesh generation 97 | s_x1 = sym_linspace(obj, obj.x_min, obj.x_max, obj.n_mesh_x); 98 | s_x2 = sym_linspace(obj, obj.x_min, obj.x_max, obj.n_mesh_x); 99 | s_x3 = sym_linspace(obj, obj.x_min, obj.x_max, obj.n_mesh_x); 100 | obj.n_mesh_x = length(s_x1); 101 | s_v1 = sym_linspace(obj, obj.v_min, obj.v_max, obj.n_mesh_v); 102 | s_v2 = sym_linspace(obj, obj.v_min, obj.v_max, obj.n_mesh_v); 103 | s_v3 = sym_linspace(obj, obj.v_min, obj.v_max, obj.n_mesh_v); 104 | obj.n_mesh_v = length(s_v1); 105 | 106 | 107 | %% initialization 108 | %states, controls 109 | [X1,V1,U1] = ndgrid(s_x1,s_v1,obj.U_vector); 110 | [X2,V2,U2] = ndgrid(s_x2,s_v2,obj.U_vector); 111 | [X3,V3,U3] = ndgrid(s_x3,s_v3,obj.U_vector); 112 | 113 | J_current = @(x,v,U,Qx,Qv,R)(Qx * x.^2 + Qv * v.^2 + R*U.^2); 114 | %calculating J fixed 115 | J_current_1 = J_current(X1,V1,U1,obj.Qx1,obj.Qv1,obj.R1); 116 | F1 = griddedInterpolant({s_x1,s_v1},zeros(obj.n_mesh_x,obj.n_mesh_v),'linear'); 117 | 118 | J_current_2 = J_current(X2,V2,U2,obj.Qx2,obj.Qv2,obj.R2); 119 | F2 = griddedInterpolant({s_x2,s_v2},zeros(obj.n_mesh_x,obj.n_mesh_v),'linear'); 120 | 121 | J_current_3 = J_current(X3,V3,U3,obj.Qx3,obj.Qv3,obj.R3); 122 | F3 = griddedInterpolant({s_x3,s_v3},zeros(obj.n_mesh_x,obj.n_mesh_v),'linear'); 123 | 124 | %calculating next stage states 125 | % 126 | [x1_next,v1_next] = next_stage_states_simplified(obj, X1, V1, U1, obj.h); 127 | [x2_next,v2_next] = next_stage_states_simplified(obj, X2, V2, U2, obj.h); 128 | [x3_next,v3_next] = next_stage_states_simplified(obj, X3, V3, U3, obj.h); 129 | 130 | %beginning (reverse) stage calculations 131 | whandle = waitbar(0,'Calculation in Progress...'); 132 | for k_s = obj.N_stage-1:-1:1 133 | tic 134 | % C = F1(w1_next,t1_next); 135 | [F1.Values, U1_idx] = min( J_current_1 + F1(x1_next,v1_next), [], 3); 136 | [F2.Values, U2_idx] = min( J_current_2 + F2(x2_next,v2_next), [], 3); 137 | [F3.Values, U3_idx] = min( J_current_3 + F3(x3_next,v3_next), [], 3); 138 | 139 | waitbar( 1 - k_s/obj.N_stage, whandle); 140 | fprintf('step %d - %f seconds\n', k_s, toc) 141 | end 142 | 143 | %set U* Optimal, replace Fi itself to save memory 144 | obj.U1_Opt = griddedInterpolant({s_x1,s_v1}, obj.U_vector(U1_idx),'nearest'); 145 | obj.U2_Opt = griddedInterpolant({s_x2,s_v2}, obj.U_vector(U2_idx),'nearest'); 146 | obj.U3_Opt = griddedInterpolant({s_x3,s_v3}, obj.U_vector(U3_idx),'nearest'); 147 | close(whandle) 148 | fprintf('stage calculation complete!\n') 149 | 150 | end 151 | 152 | function [x_next,v_next] = next_stage_states_simplified(obj, X, V, U, h) 153 | x_next = RK4_x(obj, X, V, h); 154 | v_next = RK4_v(obj, V, U, h); 155 | end 156 | 157 | function X2 = RK4_x(obj, X1, V, h) 158 | % Runge-Kutta - 4th order 159 | % h = dt; 160 | k1 = xdynamics(obj, V); 161 | k2 = xdynamics(obj,(V + k1*h/2)); 162 | k3 = xdynamics(obj,(V + k2*h/2)); 163 | k4 = xdynamics(obj,(V + k3*h)); 164 | 165 | X2 = X1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 166 | 167 | end 168 | 169 | function x_dot = xdynamics(~,v) 170 | x_dot = v; 171 | end 172 | 173 | function V2 = RK4_v(obj, V1, U, h) % does not need RK4, ki's are equal 174 | % Runge-Kutta - 4th order 175 | % h = dt; 176 | k1 = vdynamics(obj, V1 , U); 177 | k2 = vdynamics(obj,(V1 + k1*h/2), U); 178 | k3 = vdynamics(obj,(V1 + k2*h/2), U); 179 | k4 = vdynamics(obj,(V1 + k3*h), U); 180 | 181 | V2 = V1 + h*(k1 + 2*k2 + 2*k3 + k4)/6; 182 | end 183 | 184 | function v_dot = vdynamics(obj,~, U) 185 | v_dot = U/obj.Mass; 186 | end 187 | 188 | 189 | function get_optimal_path(obj) 190 | %% 191 | global mu 192 | mu = 398600; 193 | 194 | % Prescribed initial state vector of chaser B in the co-moving frame: 195 | dr0 = [-1 0 0]; 196 | dv0 = [ 0 0 0]; 197 | y0 = [dr0 dv0]'; 198 | 199 | tf = obj.T_final; 200 | %...End input data 201 | 202 | %...Calculate the target's initial state vector using Algorithm 4.5: 203 | [R0,V0] = get_target_R0V0(obj); 204 | 205 | %% 206 | N = ceil(tf/obj.h); 207 | tspan = 0:obj.h:tf; 208 | X_ode45 = zeros(6, N); 209 | F_Opt_history = zeros(3, N); 210 | X_ode45(:,1) = y0; 211 | tic 212 | for k_stage=1:N-1 213 | %determine U from X_stage 214 | X_stage = X_ode45(:,k_stage); 215 | a_x = obj.U1_Opt(X_stage(1), X_stage(4)); %x1,v1 216 | a_y = obj.U2_Opt(X_stage(2), X_stage(5)); %x2,v2 217 | a_z = obj.U3_Opt(X_stage(3), X_stage(6)); %x3,v3 218 | F_Opt_history(:,k_stage) = [a_x;a_y;a_z]; 219 | % variables used in nested differential equation function 220 | 221 | % 222 | [~,X_temp] = rkf45(@rates,[tspan(k_stage), tspan(k_stage+1)], X_ode45(:,k_stage)); 223 | X_ode45(:,k_stage+1) = X_temp(end,:)'; 224 | end 225 | toc 226 | 227 | T_ode45 = tspan(1:end-1)'; 228 | 229 | %plot states x 230 | figure 231 | hold on 232 | grid on 233 | for i=1:3 234 | plot(T_ode45,X_ode45(i,:)) 235 | end 236 | legend('x1','x2','x3') 237 | 238 | %plot states v 239 | figure 240 | hold on 241 | grid on 242 | for i=4:6 243 | plot(T_ode45,X_ode45(i,:)) 244 | end 245 | legend('v1','v2','v3') 246 | 247 | %plot controls 248 | figure 249 | hold on 250 | grid on 251 | for i=1:3 252 | plot(T_ode45,F_Opt_history(i,:)) 253 | end 254 | legend('u1','u2','u3') 255 | 256 | 257 | 258 | 259 | function dydt = rates(t,y) 260 | % ~~~~~~~~~~~~~~~~~~~~~~~~ 261 | %{ 262 | This function computes the components of f(t,y) in Equation 7.36. 263 | 264 | t - time 265 | f - column vector containing the relative position and 266 | velocity vectors of B at time t 267 | R, V - updated state vector of A at time t 268 | X, Y, Z - components of R 269 | VX, VY, VZ - components of V 270 | R_ - magnitude of R 271 | RdotV - dot product of R and V 272 | h - magnitude of the specific angular momentum of A 273 | 274 | dx , dy , dz - components of the relative position vector of B 275 | dvx, dvy, dvz - components of the relative velocity vector of B 276 | dax, day, daz - components of the relative acceleration vector of B 277 | dydt - column vector containing the relative velocity 278 | and acceleration components of B at time t 279 | 280 | User M-function required: rv_from_r0v0 281 | %} 282 | % ------------------------ 283 | %...Update the state vector of the target orbit using Algorithm 3.4: 284 | 285 | % X = R(1); Y = R(2); Z = R(3); 286 | % VX = V(1); VY = V(2); VZ = V(3); 287 | % 288 | [R,V] = update_RV_target(obj, R0, V0, t); 289 | norm_R = (R*R')^.5; %norm R 290 | RdotV = sum(R.*V); %dot product 291 | crossRV = [R(2).*V(3)-R(3).*V(2); % cross product of R and V 292 | R(3).*V(1)-R(1).*V(3); 293 | R(1).*V(2)-R(2).*V(1)]; 294 | H = (crossRV'*crossRV)^.5 ; %norm(crossRV); 295 | 296 | dx = y(1); 297 | dy = y(2); 298 | dz = y(3); 299 | 300 | dvx = y(4); 301 | dvy = y(5); 302 | dvz = y(6); 303 | 304 | dax = (2*mu/norm_R^3 + H^2/norm_R^4)*dx - 2*RdotV/norm_R^4*H*dy + 2*H/norm_R^2*dvy + a_x; 305 | day = -(mu/norm_R^3 - H^2/norm_R^4)*dy + 2*RdotV/norm_R^4*H*dx - 2*H/norm_R^2*dvx + a_y; 306 | daz = -mu/norm_R^3*dz + a_z; 307 | 308 | dydt = [dvx dvy dvz dax day daz]'; 309 | end %rates 310 | 311 | end 312 | 313 | function [R0,V0] = get_target_R0V0(obj) 314 | global mu 315 | RE = 6378; 316 | %...Input data: 317 | % Prescribed initial orbital parameters of target A: 318 | rp = RE + 300; 319 | e = 0.1; 320 | i = 0; 321 | RA = 0; 322 | omega = 0; 323 | theta = 0; 324 | % Additional computed parameters: 325 | ra = rp*(1 + e)/(1 - e); 326 | h_ = sqrt(2*mu*rp*ra/(ra + rp)); 327 | a = (rp + ra)/2; 328 | T = 2*pi/sqrt(mu)*a^1.5; 329 | n = 2*pi/T; 330 | [R0,V0] = sv_from_coe([h_ e RA i omega theta],mu); 331 | end 332 | 333 | function [R2,V2] = update_RV_target(~,R0,V0,t) 334 | %Updates the state vectors of the target sat 335 | % mu - gravitational parameter (km^3/s^2) 336 | % R0 - initial position vector (km) 337 | % V0 - initial velocity vector (km/s) 338 | % t - elapsed time (s) 339 | % R - final position vector (km) 340 | % V - final velocity vector (km/s) 341 | global mu 342 | %...Magnitudes of R0 and V0: 343 | r0 = norm(R0); 344 | v0 = norm(V0); 345 | %...Initial radial velocity: 346 | vr0 = dot(R0, V0)/r0; 347 | %...Reciprocal of the semimajor axis (from the energy equation): 348 | alpha = 2/r0 - v0^2/mu; 349 | %...Compute the universal anomaly: 350 | x = kepler_U(t, r0, vr0, alpha); 351 | %...Compute the f and g functions: 352 | [f, g] = f_and_g(x, t, r0, alpha); 353 | %...Compute the final position vector: 354 | R2 = f*R0 + g*V0; 355 | %...Compute the magnitude of R: 356 | r2 = norm(R2); 357 | %...Compute the derivatives of f and g: 358 | [fdot, gdot] = fDot_and_gDot(x, r2, r0, alpha); 359 | %...Compute the final velocity: 360 | V2 = fdot*R0 + gdot*V0; 361 | end 362 | 363 | function v = sym_linspace(~,a,b,n) 364 | if(a>0) 365 | error('minimum states are not negative, use normal linspace') 366 | end 367 | v_1 = linspace(a,0,ceil((n)/2)+1); 368 | v_2 = linspace(0,b,ceil((n)/2)+1); 369 | v_2 = v_2(2:end); %remove first zero 370 | v = [v_1,v_2]; 371 | end 372 | end 373 | 374 | methods(Static) 375 | 376 | end 377 | 378 | end 379 | 380 | 381 | -------------------------------------------------------------------------------- /position-control/private/fDot_and_gDot.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function [fdot, gdot] = fDot_and_gDot(x, r, ro, a) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function calculates the time derivatives of the Lagrange f and g coefficients. mu - the gravitational parameter (km^3/s^2) a - reciprocal of the semimajor axis (1/km) ro - the radial position at time to (km) t - the time elapsed since initial state vector (s) r - the radial position after time t (km) x - the universal anomaly after time t (km^0.5) fdot - time derivative of the Lagrange f coefficient (1/s) gdot - time derivative of the Lagrange g coefficient (dimensionless) User M-functions required: stumpC, stumpS %} % -------------------------------------------------- global mu z = a*x^2; %...Equation 3.69c: fdot = sqrt(mu)/r/ro*(z*stumpS(z) - 1)*x; %...Equation 3.69d: gdot = 1 - x^2/r*stumpC(z); % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /position-control/private/f_and_g.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function [f, g] = f_and_g(x, t, ro, a) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function calculates the Lagrange f and g coefficients. mu - the gravitational parameter (km^3/s^2) a - reciprocal of the semimajor axis (1/km) ro - the radial position at time to (km) t - the time elapsed since ro (s) x - the universal anomaly after time t (km^0.5) f - the Lagrange f coefficient (dimensionless) g - the Lagrange g coefficient (s) User M-functions required: stumpC, stumpS %} % ---------------------------------------------- global mu z = a*x^2; %...Equation 3.69a: f = 1 - x^2/ro*stumpC(z); %...Equation 3.69b: g = t - 1/sqrt(mu)*x^3*stumpS(z); end % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /position-control/private/kepler_U.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function x = kepler_U(dt, ro, vro, a) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function uses Newton's method to solve the universal Kepler equation for the universal anomaly. mu - gravitational parameter (km^3/s^2) x - the universal anomaly (km^0.5) dt - time since x = 0 (s) ro - radial position (km) when x = 0 vro - radial velocity (km/s) when x = 0 a - reciprocal of the semimajor axis (1/km) z - auxiliary variable (z = a*x^2) C - value of Stumpff function C(z) S - value of Stumpff function S(z) n - number of iterations for convergence nMax - maximum allowable number of iterations User M-functions required: stumpC, stumpS %} % ---------------------------------------------- global mu %...Set an error tolerance and a limit on the number of iterations: error = 1.e-8; nMax = 1000; %...Starting value for x: x = sqrt(mu)*abs(a)*dt; %...Iterate on Equation 3.65 until until convergence occurs within %...the error tolerance: n = 0; ratio = 1; while abs(ratio) > error && n <= nMax n = n + 1; C = stumpC(a*x^2); S = stumpS(a*x^2); F = ro*vro/sqrt(mu)*x^2*C + (1 - a*ro)*x^3*S + ro*x - sqrt(mu)*dt; dFdx = ro*vro/sqrt(mu)*x*(1 - a*x^2*S) + (1 - a*ro)*x^2*C + ro; ratio = F/dFdx; x = x - ratio; end %...Deliver a value for x, but report that nMax was reached: if n > nMax fprintf('\n **No. iterations of Kepler''s equation = %g', n) fprintf('\n F/dFdx = %g\n', F/dFdx) end % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /position-control/private/rkf45.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 2 | function [tout, yout] = rkf45(ode_function, tspan, y0, tolerance) 3 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 4 | %{ 5 | This function uses the Runge-Kutta-Fehlberg 4(5) algorithm to 6 | integrate a system of first-order differential equations 7 | dy/dt = f(t,y). 8 | 9 | y - column vector of solutions 10 | f - column vector of the derivatives dy/dt 11 | t - time 12 | a - Fehlberg coefficients for locating the six solution 13 | points (nodes) within each time interval. 14 | b - Fehlberg coupling coefficients for computing the 15 | derivatives at each interior point 16 | c4 - Fehlberg coefficients for the fourth-order solution 17 | c5 - Fehlberg coefficients for the fifth-order solution 18 | tol - allowable truncation error 19 | ode_function - handle for user M-function in which the derivatives f 20 | are computed 21 | tspan - the vector [t0 tf] giving the time interval for the 22 | solution 23 | t0 - initial time 24 | tf - final time 25 | y0 - column vector of initial values of the vector y 26 | tout - column vector of times at which y was evaluated 27 | yout - a matrix, each row of which contains the components of y 28 | evaluated at the correponding time in tout 29 | h - time step 30 | hmin - minimum allowable time step 31 | ti - time at the beginning of a time step 32 | yi - values of y at the beginning of a time step 33 | t_inner - time within a given time step 34 | y_inner - values of y witin a given time step 35 | te - trucation error for each y at a given time step 36 | te_allowed - allowable truncation error 37 | te_max - maximum absolute value of the components of te 38 | ymax - maximum absolute value of the components of y 39 | tol - relative tolerance 40 | delta - fractional change in step size 41 | eps - unit roundoff error (the smallest number for which 42 | 1 + eps > 1) 43 | eps(x) - the smallest number such that x + eps(x) = x 44 | 45 | User M-function required: ode_function 46 | %} 47 | % --------------------------------------------------------------- 48 | 49 | a = [0 1/4 3/8 12/13 1 1/2]; 50 | 51 | b = [ 0 0 0 0 0 52 | 1/4 0 0 0 0 53 | 3/32 9/32 0 0 0 54 | 1932/2197 -7200/2197 7296/2197 0 0 55 | 439/216 -8 3680/513 -845/4104 0 56 | -8/27 2 -3544/2565 1859/4104 -11/40]; 57 | 58 | c4 = [25/216 0 1408/2565 2197/4104 -1/5 0 ]; 59 | c5 = [16/135 0 6656/12825 28561/56430 -9/50 2/55]; 60 | 61 | if nargin < 4 62 | tol = 1.e-8; 63 | else 64 | tol = tolerance; 65 | end 66 | 67 | t0 = tspan(1); 68 | tf = tspan(2); 69 | t = t0; 70 | y = y0; 71 | tout = t; 72 | yout = y'; 73 | h = (tf - t0)/100; % Assumed initial time step. 74 | 75 | while t < tf 76 | hmin = 16*eps(t); 77 | ti = t; 78 | yi = y; 79 | %...Evaluate the time derivative(s) at six points within the current 80 | % interval: 81 | for i = 1:6 82 | t_inner = ti + a(i)*h; 83 | y_inner = yi; 84 | for j = 1:i-1 85 | y_inner = y_inner + h*b(i,j)*f(:,j); 86 | end 87 | f(:,i) = feval(ode_function, t_inner, y_inner); 88 | end 89 | 90 | %...Compute the maximum truncation error: 91 | te = h*f*(c4' - c5'); % Difference between 4th and 92 | % 5th order solutions 93 | te_max = max(abs(te)); 94 | 95 | %...Compute the allowable truncation error: 96 | ymax = max(abs(y)); 97 | te_allowed = tol*max(ymax,1.0); 98 | 99 | %...Compute the fractional change in step size: 100 | delta = (te_allowed/(te_max + eps))^(1/5); 101 | 102 | %...If the truncation error is in bounds, then update the solution: 103 | if te_max <= te_allowed 104 | h = min(h, tf-t); 105 | t = t + h; 106 | y = yi + h*f*c5'; 107 | tout = [tout;t]; 108 | yout = [yout;y']; 109 | end 110 | 111 | %...Update the time step: 112 | h = min(delta*h, 4*h); 113 | if h < hmin 114 | fprintf(['\n\n Warning: Step size fell below its minimum\n'... 115 | ' allowable value (%g) at time %g.\n\n'], hmin, t) 116 | return 117 | end 118 | end 119 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 120 | -------------------------------------------------------------------------------- /position-control/private/stumpC.m: -------------------------------------------------------------------------------- 1 | function c = stumpC(z) 2 | % wwwwwwwwwwwwwwwwwwwwww 3 | %{ 4 | This function evaluates the Stumpff function C(z) according 5 | to Equation 3.53. 6 | z - input argument 7 | c - value of C(z) 8 | User M-functions required: none 9 | %} 10 | % ---------------------------------------------- 11 | if z > 0 12 | c = (1 - cos(sqrt(z)))/z; 13 | elseif z < 0 14 | c = (cosh(sqrt(-z)) - 1)/(-z); 15 | else 16 | c = 1/2; 17 | end -------------------------------------------------------------------------------- /position-control/private/stumpS.m: -------------------------------------------------------------------------------- 1 | function s = stumpS(z) 2 | % wwwwwwwwwwwwwwwwwwwwww 3 | %{ 4 | This function evaluates the Stumpff function S(z) according 5 | to Equation 3.52. 6 | z - input argument 7 | s - value of S(z) 8 | User M-functions required: none 9 | %} 10 | % ---------------------------------------------- 11 | if z > 0 12 | s = (sqrt(z) - sin(sqrt(z)))/(sqrt(z))^3; 13 | elseif z < 0 14 | s = (sinh(sqrt(-z)) - sqrt(-z))/(sqrt(-z))^3; 15 | else 16 | s = 1/6; 17 | end 18 | % wwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwww -------------------------------------------------------------------------------- /position-control/private/sv_from_coe.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ function [r, v] = sv_from_coe(coe,mu) % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ %{ This function computes the state vector (r,v) from the classical orbital elements (coe). mu - gravitational parameter (km^3;s^2) coe - orbital elements [h e RA incl w TA] where h = angular momentum (km^2/s) e = eccentricity RA = right ascension of the ascending node (rad) incl = inclination of the orbit (rad) w = argument of perigee (rad) TA = true anomaly (rad) R3_w - Rotation matrix about the z-axis through the angle w R1_i - Rotation matrix about the x-axis through the angle i R3_W - Rotation matrix about the z-axis through the angle RA Q_pX - Matrix of the transformation from perifocal to geocentric equatorial frame rp - position vector in the perifocal frame (km) vp - velocity vector in the perifocal frame (km/s) r - position vector in the geocentric equatorial frame (km) v - velocity vector in the geocentric equatorial frame (km/s) User M-functions required: none %} % ---------------------------------------------- h = coe(1); e = coe(2); RA = coe(3); incl = coe(4); w = coe(5); TA = coe(6); %...Equations 4.45 and 4.46 (rp and vp are column vectors): rp = (h^2/mu) * (1/(1 + e*cos(TA))) * (cos(TA)*[1;0;0] + sin(TA)*[0;1;0]); vp = (mu/h) * (-sin(TA)*[1;0;0] + (e + cos(TA))*[0;1;0]); %...Equation 4.34: R3_W = [ cos(RA) sin(RA) 0 -sin(RA) cos(RA) 0 0 0 1]; %...Equation 4.32: R1_i = [1 0 0 0 cos(incl) sin(incl) 0 -sin(incl) cos(incl)]; %...Equation 4.34: R3_w = [ cos(w) sin(w) 0 -sin(w) cos(w) 0 0 0 1]; %...Equation 4.49: Q_pX = (R3_w*R1_i*R3_W)'; %...Equations 4.51 (r and v are column vectors): r = Q_pX*rp; v = Q_pX*vp; %...Convert r and v into row vectors: r = r'; v = v'; end % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /test/Dynamic_Solver.asv: -------------------------------------------------------------------------------- 1 | classdef Dynamic_Solver < handle 2 | %DYNAMIC_SOLVER Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | A 7 | B 8 | H 9 | R 10 | Q 11 | N %Number of stages 12 | S %Number of state values 13 | C %Number of control values 14 | %X 15 | %U 16 | x_min 17 | x_max 18 | u_min 19 | u_max 20 | dx 21 | du 22 | u_star 23 | u_star_idx 24 | J_star 25 | J_current_state 26 | J_opt_nextstate 27 | X1_mesh 28 | X2_mesh 29 | X1_mesh_3D 30 | X2_mesh_3D 31 | X_next_M1 32 | X_next_M2 33 | U_mesh_3D 34 | J_check 35 | J_current_state_check 36 | J_opt_nextstate_check 37 | J_F_next_check 38 | X_next_M1_check 39 | X_next_M2_check 40 | checkstagesXJF 41 | end 42 | 43 | methods 44 | 45 | function obj = Dynamic_Solver() 46 | obj.checkstagesXJF = 1; 47 | obj.Q = [0.25, 0; 0, 0.05]; 48 | obj.A = [0.9974, 0.0539; -0.1078, 1.1591]; 49 | obj.B = [0.0013; 0.0539]; 50 | obj.R = 0.05; 51 | obj.N = 200; 52 | obj.S = 2; 53 | obj.C = 1; 54 | %obj.X = zeros(obj.S,obj.N); 55 | %obj.U = zeros(obj.C,obj.N); 56 | obj.dx = 100; 57 | obj.du = 1000; 58 | obj.x_max = 3; 59 | obj.x_min = -2.5; 60 | obj.u_max = 10; 61 | obj.u_min = -40; 62 | end 63 | 64 | function obj = run(obj) 65 | % K = 0 66 | % Calculate and store J*NN = h(xi(N)) for all x(N) 67 | s_r = single(linspace(obj.x_min,obj.x_max,obj.dx)); 68 | [obj.X1_mesh, obj.X2_mesh] = ndgrid(s_r, s_r); 69 | 70 | U_mesh = linspace(obj.u_min, obj.u_max, obj.du); 71 | 72 | %3d grid for voctorization in calculation of C_star_M 73 | [obj.X1_mesh_3D,obj.X2_mesh_3D,obj.U_mesh_3D] = ndgrid(s_r,s_r,U_mesh); 74 | % 75 | obj.J_star = zeros([size(obj.X1_mesh),obj.N],'single'); 76 | % obj.J_star = zeros([size(obj.X1_mesh),obj.N]); 77 | % obj.J_star = zeros([size(obj.X1_mesh),obj.N]); 78 | obj.u_star = obj.J_star; 79 | [obj.X_next_M1, obj.X_next_M2] = a_D_M(obj); 80 | obj.J_current_state = g_D(obj); 81 | obj.J_opt_nextstate = zeros(size(obj.X1_mesh)); 82 | 83 | % Increase K by 1 84 | for k=1:obj.N-1 85 | tic 86 | k_s = obj.N-k; 87 | % switch k 88 | % case {3,20,60,100,129} 89 | % hold on 90 | % for ii=1:100:obj.du 91 | % plot3(obj.X1_mesh,obj.X2_mesh,J_M(:,:,ii)) 92 | % end 93 | % keyboard 94 | % close gcf 95 | % end 96 | J_state_M(obj, k); 97 | % store UMIN in UOPT(N-k,I) 98 | obj.u_star(:,:,k_s) = U_mesh(obj.u_star_idx); 99 | fprintf('step %d - %f seconds\n', k, toc) 100 | end %end of for loop when k = N 101 | 102 | 103 | end 104 | 105 | 106 | function get_optimal_path(obj, X0) 107 | if nargin < 2 108 | X0 = [2; 1] 109 | end 110 | %Store Optimal controls, UOPT(N-k,I) and min costs, COST(N-k,I) 111 | %for all quantized state points (I = 1,2,..,S) and all stages 112 | % K = 1:N 113 | plot_k_max = obj.N; 114 | v = 1:plot_k_max; 115 | X = zeros(obj.S,obj.N); 116 | U = zeros(obj.C,obj.N); 117 | J = U; 118 | X(:,1) = X0; 119 | for k=1:obj.N-1 120 | Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 121 | obj.u_star(:,:,k),'linear'); 122 | 123 | U(k) = Fu(X(1,k),X(2,k)); 124 | 125 | % Fj = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 126 | % obj.J_star(:,:,k),'linear'); 127 | % J(k) = Fj(X(1,k),X(2,k)); 128 | 129 | X(:,k+1) = a_D(obj,X(1,k),X(2,k),U(k)); 130 | % X(:,k+1) = obj.A*X(:,k) + obj.B*U(k); 131 | 132 | end 133 | % k = k+1; 134 | % %-- Optimal Control Input u* 135 | % Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 136 | % obj.u_star(:,:,k),'linear'); 137 | % U(k) = Fu(X(1,k),X(2,k)); 138 | 139 | %-- Commented -- Cost of path 140 | %Fj = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 141 | % obj.J_star(:,:,k),'linear'); 142 | %J(k) = Fj(X(1,k),X(2,k)); 143 | 144 | %Print Optimal Controls 145 | plot(v,X(1,v)) 146 | hold on 147 | plot(v,X(2,v),'r') 148 | plot(v,U(v),'--') 149 | title('Optimal control for initial state X0') 150 | xlabel('stage - k') 151 | ylabel('state and inputs') 152 | legend('X1', 'X2', 'u*'); 153 | grid on 154 | xlim([v(1) v(end)]) 155 | 156 | end 157 | 158 | 159 | function [Xnext_M1,Xnext_M2] = a_D_M(obj) 160 | %keyboard; 161 | Xnext_M1 = obj.A(1)*obj.X1_mesh_3D + obj.A(3)*obj.X2_mesh_3D + obj.B(1)*obj.U_mesh_3D; 162 | Xnext_M2 = obj.A(2)*obj.X1_mesh_3D + obj.A(4)*obj.X2_mesh_3D + obj.B(2)*obj.U_mesh_3D; 163 | end 164 | 165 | 166 | function X1_new = a_D(obj,X1,X2,Ui) 167 | % old function used for get_optimal_path 168 | X1_new = obj.A*[X1;X2] + obj.B*Ui; 169 | end 170 | 171 | function J = g_D(obj) 172 | %J = [X1;X2]' * obj.Q * [X1;X2] + Ui' * obj.R * Ui; 173 | J = obj.Q(1)*obj.X1_mesh_3D.^2 + ... 174 | obj.Q(4)*obj.X2_mesh_3D.^2 + obj.R * obj.U_mesh_3D.^2; 175 | end 176 | 177 | function J_state_M(obj,k) 178 | F = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 179 | obj.J_opt_nextstate,'linear'); 180 | %get next state X 181 | % [X_next_M1,X_next_M2] = a_D_M(obj); 182 | %find J final for each state and control (X,U) and add it to next state 183 | %optimum J* 184 | J_F_next = F(obj.X_next_M1, obj.X_next_M2); 185 | % % Add up J's % % 186 | [obj.J_opt_nextstate, obj.u_star_idx] = ... 187 | min(J_F_next + obj.J_current_state,[],3); 188 | % % % % % % % % % % 189 | if(obj.checkstagesXJF) 190 | obj.J_current_state_check(:,:,k) = obj.J_current_state(50:55,52:57,105); 191 | obj.J_opt_nextstate_check(:,:,k) = obj.J_opt_nextstate(50:55,52:57); 192 | obj.J_F_next_check(:,:,k) = J_F_next(50:55,52:57,105); 193 | obj.X_next_M1_check(:,:,k) = obj.X_next_M1(50:55,52:57,105); 194 | obj.X_next_M2_check(:,:,k) = obj.X_next_M2(50:55,52:57,105); 195 | % obj.J_check(:,:,k) = obj.J_opt_nextstate(50:55,52:57,105); 196 | end 197 | end 198 | 199 | function compare_stages(obj, k) 200 | fprintf('%% -------------------------------------------\n') 201 | fprintf('J for stages\n') 202 | obj.J_check(:,:,k) 203 | fprintf('%% -----------------------------------------------\n') 204 | fprintf('J Current stage for stages\n') 205 | obj.J_current_state_check(:,:,k) 206 | fprintf('%% -------------------------------------------\n') 207 | fprintf('J optimum next stage for stages\n') 208 | obj.J_opt_nextstate_check(:,:,k) 209 | fprintf('%% -------------------------------------------\n') 210 | fprintf('X1 next stage\n') 211 | obj.X_next_M1_check(:,:,k) 212 | fprintf('%% -------------------------------------------\n') 213 | fprintf('X2 next stage\n') 214 | obj.X_next_M2_check(:,:,k) 215 | end 216 | 217 | function plot_u_star(this,k_s) 218 | if nargin < 2 219 | k_s = 1:this.N-2; 220 | end 221 | 222 | if length(k_s) == 1 223 | figure 224 | plot3( this.X1_mesh, this.X2_mesh, this.u_star(:,:,k_s) ) 225 | else 226 | figure 227 | p = mesh(this.X1_mesh, this.X2_mesh, this.u_star(:,:,k_s(1)) ); 228 | colormap winter 229 | % 230 | for i=2:length(k_s) 231 | k_temp = k_s(i); 232 | p.ZData = this.u_star(:,:,k_s(i)); 233 | title(['Stage ',num2str(k_temp)]); 234 | pause(0.2) 235 | end 236 | end 237 | end 238 | end 239 | 240 | methods (Static) 241 | 242 | function b = compare_data(obj1,obj2) 243 | % use this function to compare saved datas 244 | % check J* matrix 245 | if( isempty(obj1.J_star) || isempty(obj2.J_star) ) 246 | error('stop throwing empty data at me') 247 | end 248 | %compare 249 | if( isequal(obj1.J_star, obj2.J_star) ) 250 | disp('J_star matrices comparison -- Match!') 251 | b = true; 252 | else 253 | warning('J_star matrices -- Do NOT match') 254 | b = false; 255 | end 256 | end 257 | % end 258 | end 259 | 260 | 261 | end 262 | 263 | -------------------------------------------------------------------------------- /test/Dynamic_Solver.m: -------------------------------------------------------------------------------- 1 | classdef Dynamic_Solver < handle 2 | %DYNAMIC_SOLVER Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | A 7 | B 8 | H 9 | R 10 | Q 11 | N %Number of stages 12 | S %Number of state values 13 | C %Number of control values 14 | %X 15 | %U 16 | x_min 17 | x_max 18 | u_min 19 | u_max 20 | s_r 21 | dx 22 | du 23 | u_star 24 | u_star_idx 25 | J_star 26 | J_current_state 27 | F 28 | %J_opt_nextstate 29 | X1_mesh 30 | X2_mesh 31 | X1_mesh_3D 32 | X2_mesh_3D 33 | X_next_M1 34 | X_next_M2 35 | U_mesh_3D 36 | J_check 37 | J_current_state_check 38 | J_opt_nextstate_check 39 | J_F_next_check 40 | X_next_M1_check 41 | X_next_M2_check 42 | checkstagesXJF 43 | end 44 | 45 | methods 46 | 47 | function obj = Dynamic_Solver() 48 | obj.checkstagesXJF = 1; 49 | obj.Q = [0.25, 0; 0, 0.05]; 50 | obj.A = [0.9974, 0.0539; -0.1078, 1.1591]; 51 | obj.B = [0.0013; 0.0539]; 52 | obj.R = 0.05; 53 | obj.N = 200; 54 | obj.S = 2; 55 | obj.C = 1; 56 | %obj.X = zeros(obj.S,obj.N); 57 | %obj.U = zeros(obj.C,obj.N); 58 | obj.dx = 100; 59 | obj.du = 1000; 60 | obj.x_max = 3; 61 | obj.x_min = -2.5; 62 | obj.u_max = 10; 63 | obj.u_min = -40; 64 | end 65 | 66 | function obj = run(obj) 67 | % K = 0 68 | % Calculate and store J*NN = h(xi(N)) for all x(N) 69 | obj.s_r = single(linspace(obj.x_min,obj.x_max,obj.dx)); 70 | [obj.X1_mesh, obj.X2_mesh] = ndgrid(obj.s_r, obj.s_r); 71 | 72 | U_mesh = linspace(obj.u_min, obj.u_max, obj.du); 73 | 74 | %3d grid for voctorization in calculation of C_star_M 75 | [obj.X1_mesh_3D,obj.X2_mesh_3D,obj.U_mesh_3D] = ndgrid(obj.s_r,obj.s_r,U_mesh); 76 | % 77 | obj.J_star = zeros([size(obj.X1_mesh),obj.N],'single'); 78 | % obj.J_star = zeros([size(obj.X1_mesh),obj.N]); 79 | % obj.J_star = zeros([size(obj.X1_mesh),obj.N]); 80 | obj.u_star = obj.J_star; 81 | [obj.X_next_M1, obj.X_next_M2] = a_D_M(obj); 82 | obj.J_current_state = g_D(obj); 83 | obj.F = griddedInterpolant({obj.s_r,obj.s_r},... 84 | zeros(size(obj.X1_mesh)),'linear'); 85 | % Increase K by 1 86 | for k=1:obj.N-1 87 | tic 88 | k_s = obj.N-k; 89 | % switch k 90 | % case {3,20,60,100,129} 91 | % hold on 92 | % for ii=1:100:obj.du 93 | % plot3(obj.X1_mesh,obj.X2_mesh,J_M(:,:,ii)) 94 | % end 95 | % keyboard 96 | % close gcf 97 | % end 98 | J_state_M(obj, k); 99 | % store UMIN in UOPT(N-k,I) 100 | obj.u_star(:,:,k_s) = U_mesh(obj.u_star_idx); 101 | fprintf('step %d - %f seconds\n', k, toc) 102 | end %end of for loop when k = N 103 | 104 | 105 | end 106 | 107 | 108 | function get_optimal_path(obj, X0, mode, ssu_num) 109 | if nargin < 3 110 | X0 = [2; 1] 111 | mode = 'Nssu'; % steady state u_star matrix at stage 1 112 | ssu_num = 1; 113 | end 114 | %Store Optimal controls, UOPT(N-k,I) and min costs, COST(N-k,I) 115 | %for all quantized state points (I = 1,2,..,S) and all stages 116 | % K = 1:N 117 | plot_k_max = obj.N; 118 | v = 1:plot_k_max; 119 | X = zeros(obj.S,obj.N); 120 | U = zeros(obj.C,obj.N); 121 | J = U; 122 | X(:,1) = X0; 123 | USTAR_OPT = obj.u_star(:,:,1); 124 | USM = obj.u_star(:,:,ssu_num); 125 | tol = sum(sum(USTAR_OPT - USM).^2); 126 | for k=1:obj.N-1 127 | if strcmp(mode,'ssu') 128 | USM = obj.u_star(:,:,ssu_num); 129 | else 130 | USM = obj.u_star(:,:,k); 131 | end 132 | Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 133 | USM,'linear'); 134 | 135 | U(k) = Fu(X(1,k),X(2,k)); 136 | 137 | 138 | % Fj = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 139 | % obj.J_star(:,:,k),'linear'); 140 | % J(k) = Fj(X(1,k),X(2,k)); 141 | 142 | X(:,k+1) = a_D(obj,X(1,k),X(2,k),U(k)); 143 | % X(:,k+1) = obj.A*X(:,k) + obj.B*U(k); 144 | 145 | end 146 | % k = k+1; 147 | % %-- Optimal Control Input u* 148 | % Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 149 | % obj.u_star(:,:,k),'linear'); 150 | % U(k) = Fu(X(1,k),X(2,k)); 151 | 152 | %-- Commented -- Cost of path 153 | %Fj = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 154 | % obj.J_star(:,:,k),'linear'); 155 | %J(k) = Fj(X(1,k),X(2,k)); 156 | 157 | %Print Optimal Controls 158 | plot(v,X(1,v)) 159 | hold on 160 | plot(v,X(2,v),'r') 161 | plot(v,U(v),'--') 162 | title('Optimal control for initial state X0') 163 | xlabel('stage - k') 164 | ylabel('state and inputs') 165 | legend('X1', 'X2', 'u*'); 166 | grid on 167 | xlim([v(1) v(end)]) 168 | if(strcmp(mode,'ssu')) 169 | fprintf('sum of all U deviations from stage 1 Matrix: %.5f\n',tol) 170 | % calculate deviation of first U 171 | Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 172 | USM,'linear'); 173 | U_first_actual = Fu(X(1,1),X(2,1)); 174 | Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 175 | USTAR_OPT,'linear'); 176 | U_first_SSU = Fu(X(1,1),X(2,1)); 177 | err_U_first = abs(U_first_SSU - U_first_actual); 178 | fprintf('deviation of first U from stage 1 Matrix: %.5f\n',err_U_first) 179 | 180 | end 181 | end 182 | 183 | 184 | function [Xnext_M1,Xnext_M2] = a_D_M(obj) 185 | %keyboard; 186 | Xnext_M1 = obj.A(1)*obj.X1_mesh_3D + obj.A(3)*obj.X2_mesh_3D + obj.B(1)*obj.U_mesh_3D; 187 | Xnext_M2 = obj.A(2)*obj.X1_mesh_3D + obj.A(4)*obj.X2_mesh_3D + obj.B(2)*obj.U_mesh_3D; 188 | end 189 | 190 | 191 | function X1_new = a_D(obj,X1,X2,Ui) 192 | % old function used for get_optimal_path 193 | X1_new = obj.A*[X1;X2] + obj.B*Ui; 194 | end 195 | 196 | function J = g_D(obj) 197 | %J = [X1;X2]' * obj.Q * [X1;X2] + Ui' * obj.R * Ui; 198 | J = obj.Q(1)*obj.X1_mesh_3D.^2 + ... 199 | obj.Q(4)*obj.X2_mesh_3D.^2 + obj.R * obj.U_mesh_3D.^2; 200 | end 201 | 202 | function J_state_M(obj,k) 203 | %get next state X 204 | % [X_next_M1,X_next_M2] = a_D_M(obj); 205 | %find J final for each state and control (X,U) and add it to next state 206 | %optimum J* 207 | J_F_next = obj.F(obj.X_next_M1, obj.X_next_M2); 208 | % % Add up J's % % 209 | [obj.F.Values, obj.u_star_idx] = ... 210 | min(J_F_next + obj.J_current_state,[],3); 211 | % % % % % % % % % % 212 | if(obj.checkstagesXJF) 213 | obj.J_current_state_check(:,:,k) = obj.J_current_state(50:55,52:57,105); 214 | % obj.J_opt_nextstate_check(:,:,k) = obj.J_opt_nextstate(50:55,52:57); 215 | % obj.J_F_next_check(:,:,k) = J_F_next(50:55,52:57,105); 216 | obj.X_next_M1_check(:,:,k) = obj.X_next_M1(50:55,52:57,105); 217 | obj.X_next_M2_check(:,:,k) = obj.X_next_M2(50:55,52:57,105); 218 | % obj.J_check(:,:,k) = obj.J_opt_nextstate(50:55,52:57,105); 219 | end 220 | end 221 | 222 | function compare_stages(obj, k) 223 | fprintf('%% -------------------------------------------\n') 224 | fprintf('J for stages\n') 225 | obj.J_check(:,:,k) 226 | fprintf('%% -----------------------------------------------\n') 227 | fprintf('J Current stage for stages\n') 228 | obj.J_current_state_check(:,:,k) 229 | fprintf('%% -------------------------------------------\n') 230 | fprintf('J optimum next stage for stages\n') 231 | obj.J_opt_nextstate_check(:,:,k) 232 | fprintf('%% -------------------------------------------\n') 233 | fprintf('X1 next stage\n') 234 | obj.X_next_M1_check(:,:,k) 235 | fprintf('%% -------------------------------------------\n') 236 | fprintf('X2 next stage\n') 237 | obj.X_next_M2_check(:,:,k) 238 | end 239 | 240 | function plot_u_star(this,k_s) 241 | if nargin < 2 242 | k_s = 1:this.N-2; 243 | end 244 | 245 | if length(k_s) == 1 246 | figure 247 | plot3( this.X1_mesh, this.X2_mesh, this.u_star(:,:,k_s) ) 248 | else 249 | figure 250 | p = mesh(this.X1_mesh, this.X2_mesh, this.u_star(:,:,k_s(1)) ); 251 | colormap winter 252 | % not allowing axis limits to change automatically 253 | axis manual 254 | for i=2:length(k_s) 255 | k_temp = k_s(i); 256 | p.ZData = this.u_star(:,:,k_s(i)); 257 | title(['Stage ',num2str(k_temp)]); 258 | pause(0.2) 259 | end 260 | end 261 | end 262 | end 263 | 264 | methods (Static) 265 | 266 | function b = compare_data(obj1,obj2) 267 | % use this function to compare saved datas 268 | % check J* matrix 269 | if( isempty(obj1.J_star) || isempty(obj2.J_star) ) 270 | error('stop throwing empty data at me') 271 | end 272 | %compare 273 | if( isequal(obj1.J_star, obj2.J_star) ) 274 | disp('J_star matrices comparison -- Match!') 275 | b = true; 276 | else 277 | warning('J_star matrices -- Do NOT match') 278 | b = false; 279 | end 280 | end 281 | % end 282 | end 283 | 284 | 285 | end 286 | 287 | -------------------------------------------------------------------------------- /test/obj_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abdolrezat/Optimal-Control-Dynamic-Programming/9d079355544de7c4c0300a5ff8a400a069656731/test/obj_1.mat -------------------------------------------------------------------------------- /test/obj_1.txt: -------------------------------------------------------------------------------- 1 | function obj = Dynamic_Solver() 2 | obj.Q = [0.25, 0; 0, 0.05]; 3 | obj.A = [0.9974, 0.0539; -0.1078, 1.1591]; 4 | obj.B = [0.0013; 0.0539]; 5 | obj.R = 0.05; 6 | obj.N = 130; 7 | obj.S = 2; 8 | obj.C = 1; 9 | %obj.X = zeros(obj.S,obj.N); 10 | %obj.U = zeros(obj.C,obj.N); 11 | obj.dx = 35; 12 | obj.du = 100; 13 | obj.x_max = 3; 14 | obj.x_min = -2.5; 15 | obj.u_max = 10; 16 | obj.u_min = -40; 17 | end -------------------------------------------------------------------------------- /test/result-obj_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abdolrezat/Optimal-Control-Dynamic-Programming/9d079355544de7c4c0300a5ff8a400a069656731/test/result-obj_1.png -------------------------------------------------------------------------------- /test/target-obj_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abdolrezat/Optimal-Control-Dynamic-Programming/9d079355544de7c4c0300a5ff8a400a069656731/test/target-obj_1.png -------------------------------------------------------------------------------- /test/test_coder.m: -------------------------------------------------------------------------------- 1 | function test_coder() 2 | obj.Q = [0.25, 0; 0, 0.05]; 3 | obj.A = [0.9974, 0.0539; -0.1078, 1.1591]; 4 | obj.B = [0.0013; 0.0539]; 5 | obj.R = 0.05; 6 | obj.N = 130; 7 | obj.S = 2; 8 | obj.C = 1; 9 | %obj.X = zeros(obj.S,obj.N); 10 | %obj.U = zeros(obj.C,obj.N); 11 | obj.dx = 100; 12 | obj.du = 1000; 13 | obj.x_max = 3; 14 | obj.x_min = -2.5; 15 | obj.u_max = 10; 16 | obj.u_min = -40; 17 | % K = 0 18 | % Calculate and store J*NN = h(xi(N)) for all x(N) 19 | s_r = linspace(obj.x_min,obj.x_max,obj.dx); 20 | [obj.X1_mesh, obj.X2_mesh] = ndgrid(s_r, s_r); 21 | U_mesh = linspace(obj.u_min, obj.u_max, obj.du); 22 | %3d grid for voctorization in calculation of C_star_M 23 | [obj.X1_mesh_3D,obj.X2_mesh_3D,obj.U_mesh_3D] = ndgrid(s_r,s_r,U_mesh); 24 | % 25 | obj.J_star = zeros([size(obj.X1_mesh),obj.N]); 26 | obj.u_star = obj.J_star; 27 | % Increase K by 1 28 | for k=1:obj.N-1 29 | 30 | k_s = obj.N-k; 31 | J_M = J_state_M(obj, k); 32 | [obj.J_star(:,:,k_s),u_star_idx] = min(J_M,[],3); 33 | % store UMIN in UOPT(N-k,I) 34 | obj.u_star(:,:,k_s) = U_mesh(u_star_idx); 35 | fprintf('step %d - %f seconds\n', k) 36 | end %end of for loop when k = N 37 | get_optimal_path(obj) 38 | end 39 | function get_optimal_path(obj, X0) 40 | if nargin < 2 41 | X0 = [2; 1] 42 | end 43 | %Store Optimal controls, UOPT(N-k,I) and min costs, COST(N-k,I) 44 | %for all quantized state points (I = 1,2,..,S) and all stages 45 | % K = 1:N 46 | plot_k_max = obj.N; 47 | v = 1:plot_k_max; 48 | X = zeros(obj.S,obj.N); 49 | U = zeros(obj.C,obj.N); 50 | J = U; 51 | X(:,1) = X0; 52 | for k=1:obj.N-1 53 | 54 | Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 55 | obj.u_star(:,:,k),'linear'); 56 | 57 | U(k) = Fu(X(1,k),X(2,k)); 58 | 59 | Fj = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 60 | obj.J_star(:,:,k),'linear'); 61 | J(k) = Fj(X(1,k),X(2,k)); 62 | 63 | X(:,k+1) = a_D(obj,X(1,k),X(2,k),U(k)); 64 | % X(:,k+1) = obj.A*X(:,k) + obj.B*U(k); 65 | end 66 | k = k+1; 67 | %-- Optimal Control Input u* 68 | Fu = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 69 | obj.u_star(:,:,k),'linear'); 70 | U(k) = Fu(X(1,k),X(2,k)); 71 | 72 | %-- Commented -- Cost of path 73 | %Fj = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 74 | % obj.J_star(:,:,k),'linear'); 75 | %J(k) = Fj(X(1,k),X(2,k)); 76 | 77 | %Print Optimal Controls 78 | plot(v,X(1,v)) 79 | hold on 80 | plot(v,X(2,v),'r') 81 | plot(v,U(v),'--') 82 | title('Optimal control for initial state X0') 83 | xlabel('stage - k') 84 | ylabel('state and inputs') 85 | grid on 86 | xlim([v(1) v(end)]) 87 | 88 | end 89 | 90 | 91 | function [Xnext_M1,Xnext_M2] = a_D_M(obj) 92 | %keyboard; 93 | Xnext_M1 = obj.A(1)*obj.X1_mesh_3D + obj.A(3)*obj.X2_mesh_3D + obj.B(1)*obj.U_mesh_3D; 94 | Xnext_M2 = obj.A(2)*obj.X1_mesh_3D + obj.A(4)*obj.X2_mesh_3D + obj.B(2)*obj.U_mesh_3D; 95 | end 96 | 97 | 98 | function X1_new = a_D(obj,X1,X2,Ui) 99 | % old function used for get_optimal_path 100 | X1_new = obj.A*[X1;X2] + obj.B*Ui; 101 | end 102 | 103 | function J = g_D(obj) 104 | %J = [X1;X2]' * obj.Q * [X1;X2] + Ui' * obj.R * Ui; 105 | J = obj.Q(1)*obj.X1_mesh_3D.^2 + ... 106 | obj.Q(4)*obj.X2_mesh_3D.^2 + obj.R * obj.U_mesh_3D.^2; 107 | end 108 | 109 | function J = J_state_M(obj,k) 110 | F = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 111 | obj.J_star(:,:,obj.N-k+1),'linear'); 112 | %get next state X 113 | [X_next_M1,X_next_M2] = a_D_M(obj); 114 | %find J final for each state and control (X,U) and add it to next state 115 | %optimum J* 116 | J = F(X_next_M1,X_next_M2) + g_D(obj); 117 | end 118 | 119 | 120 | function b = compare_data(obj1,obj2) 121 | % use this function to compare saved datas 122 | % check J* matrix 123 | if( isempty(obj1.J_star) || isempty(obj2.J_star) ) 124 | error('stop throwing empty data at me') 125 | end 126 | %compare 127 | if( isequal(obj1.J_star, obj2.J_star) ) 128 | disp('J_star matrices comparison -- Match!') 129 | b = true; 130 | else 131 | %warning('J_star matrices -- Do NOT match') 132 | b = false; 133 | end 134 | end -------------------------------------------------------------------------------- /test/test_griddedInterp.m: -------------------------------------------------------------------------------- 1 | %% test gridded interpolant see if it works as desired (works) 2 | obj = Dynamic_Solver; 3 | s_r = linspace(obj.x_min,obj.x_max,obj.dx); 4 | [obj.X1_mesh, obj.X2_mesh] = ndgrid(s_r, s_r); 5 | U_mesh = linspace(obj.u_max, obj.u_min,obj.du); 6 | obj.J_star = zeros([size(obj.X1_mesh),obj.N]); 7 | obj.u_star = obj.J_star; 8 | k = 1; 9 | 10 | %make a surface 11 | obj.J_star(:,:,obj.N-k+1) = 2.* obj.X1_mesh .* obj.X2_mesh + obj.X2_mesh 12 | obj.J_star(:,:,end) %display surface Z 13 | F = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 14 | obj.J_star(:,:,obj.N-k+1),'linear'); 15 | 16 | plot3(obj.X1_mesh, obj.X2_mesh, obj.J_star(:,:,end)) 17 | hold on 18 | plot3(0,0,F(0,0),'*') %plot a point 19 | p = @(a,b)(plot3(a,b,F(a,b),'*r')); %function to plot points 20 | p([7 5 3 3 1 -4 -5],[10 8 5 3 -1 -2 -5]) %plot random points see if they fit the surface 21 | %it works 22 | 23 | %test with only grid vectors {s_r,s_r} 24 | F22 = griddedInterpolant({s_r,s_r}, obj.J_star(:,:,end)); 25 | 26 | % test with one grid vector and one mesh grid 27 | J = obj.J_star(:,:,end); 28 | %F33 = griddedInterpolant({s_r},obj.X1_mesh, J); 29 | %test memory 30 | whos F22 J 31 | %test type 'single' variables 32 | FS = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 33 | single(J),'linear'); 34 | whos FS %half the size of doubles 35 | 36 | %test one Y(1xN) vector 37 | %FY = griddedInterpolant({s_r,s_r}, J(:)') 38 | %FXY = griddedInterpolant(obj.X1_mesh(:), obj.X2_mesh(:), J(:),'linear') 39 | 40 | %test performance of 'nearest' vs. 'linear' 41 | 42 | FN = griddedInterpolant(obj.X1_mesh, obj.X2_mesh,... 43 | obj.J_star(:,:,obj.N-k+1),'nearest'); 44 | 45 | pN = @(a,b)(plot3(a,b,FN(a,b),'*b')); %function to plot points 46 | pN([7 5 3 3 1 -4 -5],[10 8 5 3 -1 -2 -5]) %plot random points see if they fit the surface 47 | 48 | 49 | tic 50 | X = J; 51 | for ii=1:10000 52 | X = F(obj.X1_mesh, obj.X2_mesh); 53 | end 54 | toc 55 | tic 56 | X = J; 57 | for ii=1:10000 58 | X = FN(obj.X1_mesh, obj.X2_mesh); 59 | end 60 | toc 61 | 62 | -------------------------------------------------------------------------------- /test/test_performance_Interpolant.m: -------------------------------------------------------------------------------- 1 | % test between different 2d interpolant functions Interp2 and 2 | % griddedinterpolant performances. result: predefined griddedinterpolant is the 3 | % best method. 4 | %% Choose settings 5 | %low dim 6 | N1 = 1000; 7 | N2 = 5000; 8 | N3 = 5000; 9 | 10 | 11 | [X1,X2] = meshgrid(-100:0.2:100,-100:0.2:100); 12 | J = X1.^2 + 78*X2.^2; 13 | plot3(X1,X2,J) 14 | 15 | 16 | tic 17 | for ii= 1:N1 18 | j2 = interp2(X1,X2,J,X1(ii),X2(ii),'linear'); 19 | end 20 | toc 21 | 22 | X1 = X1'; 23 | X2 = X2'; 24 | 25 | tic 26 | for jj = 1:N2 27 | F = griddedInterpolant(X1,X2,J,'linear'); 28 | j3 = F(X1(jj),X2(jj)); 29 | end 30 | toc 31 | 32 | %% new idea 33 | F = griddedInterpolant(X1,X2,J,'linear'); 34 | tic 35 | for kk = 1:N3 36 | J4(kk) = F(X1(kk),X2(kk)); 37 | end 38 | toc 39 | hold on 40 | 41 | plot3(X1(1:kk), X2(1:kk), J4(1:kk), '*') -------------------------------------------------------------------------------- /test/test_performance_X_inline.m: -------------------------------------------------------------------------------- 1 | %tests the performance difference if X1(i,ii) and other matrices are 2 | %implemented directly in loops instead of allocation to another variable 3 | %e.g. x1 = X1(i,ii); 4 | % Result: time is about 50% reduced when the matrix element is 5 | [X1, X2] = ndgrid(1:100,1:100); 6 | U = 1:1000; 7 | 8 | J = zeros(100,100,1000); 9 | 10 | for k = 1:10 11 | tic 12 | for i=1:100 13 | for ii=1:100 14 | COSTMIN = Inf; 15 | for j = 1:1000 16 | C_star = X1(i,ii)*2 + 2*X2(i,ii)^2 + 300*U(j); 17 | if C_star < COSTMIN 18 | COSTMIN = C_star; 19 | end 20 | end 21 | J(i,ii,k) = COSTMIN; 22 | end 23 | end 24 | fprintf('test 1: step %d - %f seconds\n', k, toc) 25 | end 26 | 27 | %% ------------------------------------ Second Test ----------------------------- %% 28 | 29 | 30 | [X1, X2] = ndgrid(1:100,1:100); 31 | U = 1:1000; 32 | 33 | J = zeros(100,100,1000); 34 | 35 | for k = 1:10 36 | tic 37 | for i=1:100 38 | x1 = X1(i,ii); 39 | for ii=1:100 40 | x2 = X2(i,ii); 41 | COSTMIN = Inf; 42 | for j = 1:1000 43 | u = U(j); 44 | C_star = x1*2 + 2*x2^2 + 300*u; 45 | if C_star < COSTMIN 46 | COSTMIN = C_star; 47 | end 48 | end 49 | J(i,ii,k) = COSTMIN; 50 | end 51 | end 52 | fprintf('test 2: step %d - %f seconds\n', k, toc) 53 | end -------------------------------------------------------------------------------- /test/test_performance_find.m: -------------------------------------------------------------------------------- 1 | %we'll test the performance of Two approaches: 1.using "elementwise 2 | %multiplication + find()" to get the min of a 100x100x1000 J matrix and 2. 3 | %using three (for) loops the usual approach 4 | Q = [0.25, 0; 0, 0.05]; 5 | A = [0.9974, 0.0539; -0.1078, 1.1591]; 6 | B = [0.0013; 0.0539]; 7 | R = 0.05; 8 | N = 30; 9 | 10 | [X1, X2] = ndgrid(1:N,1:N); 11 | U = 1:1000; 12 | %% ----------------- Non-Vectorized Implementation ---------------------- %% 13 | 14 | tic 15 | for i=1:N 16 | for ii=1:N 17 | x1 = X1(i,ii); 18 | x2 = X2(i,ii); 19 | COSTMIN = 10000; 20 | UMIN = 10000; 21 | for jj = U 22 | Ui = U(jj); 23 | X_next = A*[x1;x2] + B*Ui; 24 | C_star = [x1;x2]' * Q * [x1;x2] + Ui' * R * Ui; 25 | if(C_star < COSTMIN) 26 | COSTMIN = C_star; 27 | UMIN = Ui; 28 | end 29 | end 30 | % store UMIN in UOPT(N-k,I) 31 | u_star(i,ii) = UMIN; 32 | % store COSMIN in COST(N-k,i) 33 | J_star(i,ii) = COSTMIN; 34 | end 35 | end 36 | fprintf('non-vectorized operation complete in %f seconds...\n',toc) 37 | 38 | %% ----------------- Vectorized Implementation ---------------------- %% 39 | J_star_V = zeros(N,N); 40 | u_star_V = J_star_V; 41 | JV = zeros(N,N,length(U)); 42 | X_next = [(A(1)*X1 + A(3)*X2 + B(1)*Ui); 43 | (A(2)*X1 + A(4)*X2 + B(2)*Ui)]; 44 | 45 | C_star = Q(1)*X1.^2 + Q(4)*X2.^2 + R * Ui.^2; 46 | 47 | 48 | -------------------------------------------------------------------------------- /test/test_performance_ii.m: -------------------------------------------------------------------------------- 1 | % tests to assess how much performance is improved if a single line of 2 | % allocation e.g. x1 = X1(i,ii) executed in the outer loop. of course the 3 | % results will differ cause the logics are not the same 4 | 5 | [X1, X2] = ndgrid(1:100,1:100); 6 | U = 1:1000; 7 | 8 | J = zeros(100,100,1000); 9 | 10 | for k = 1:10 11 | tic 12 | for i=1:100 13 | for ii=1:100 14 | x1 = X1(i,ii); 15 | x2 = X2(i,ii); 16 | COSTMIN = Inf; 17 | for j = 1:1000 18 | u = U(j); 19 | C_star = x1*2 + 2*x2^2 + 300*u; 20 | if C_star < COSTMIN 21 | COSTMIN = C_star; 22 | end 23 | end 24 | J(i,ii,k) = COSTMIN; 25 | end 26 | end 27 | fprintf('test 1: step %d - %f seconds\n', k, toc) 28 | end 29 | 30 | %% ------------------------------------ Second Test ----------------------------- %% 31 | 32 | [X1, X2] = ndgrid(1:100,1:100); 33 | U = 1:1000; 34 | 35 | J = zeros(100,100,1000); 36 | 37 | for k = 1:10 38 | tic 39 | for i=1:100 40 | x1 = X1(i,ii); 41 | for ii=1:100 42 | x2 = X2(i,ii); 43 | COSTMIN = Inf; 44 | for j = 1:1000 45 | u = U(j); 46 | C_star = x1*2 + 2*x2^2 + 300*u; 47 | if C_star < COSTMIN 48 | COSTMIN = C_star; 49 | end 50 | end 51 | J(i,ii,k) = COSTMIN; 52 | end 53 | end 54 | fprintf('test 2: step %d - %f seconds\n', k, toc) 55 | end -------------------------------------------------------------------------------- /test/test_u_star_M.m: -------------------------------------------------------------------------------- 1 | %% Initialization 2 | D = Dynamic_Solver 3 | run(D) 4 | 5 | %% Visualize Optimum U(x1,x2) Matrix at every stage 6 | D.plot_u_star() 7 | %% find optimal path using one stage U matrix ( steady state U) 8 | D.get_optimal_path([2;1],'ssu',190) % bad 9 | D.get_optimal_path([2;1],'ssu',180) % controllable but inefficient 10 | D.get_optimal_path([2;1],'ssu',135) % near optimal control obtained at stage 1 11 | D.get_optimal_path([2;1],'ssu',128) % 12 | D.get_optimal_path([2;1],'ssu',127) % 13 | D.get_optimal_path([2;1],'ssu',30) % 14 | 15 | %% find optimal path by stepping through stage 1 to N 16 | D.get_optimal_path --------------------------------------------------------------------------------