diff --git a/lib/switch.ks b/lib/switch.ks index b60c334..f54a6f9 100644 --- a/lib/switch.ks +++ b/lib/switch.ks @@ -1,3 +1,5 @@ +local evt is 1.0e-8. + local function iterate { parameter maneuver, no. // `maneuver` is a lexicon of: @@ -19,20 +21,36 @@ local function iterate { // "fuel" - total amount of fuel in the stage // "boiloff_rate" - how quickly fuel evaporates when not being used // "dry_mass" - mass lost when vehicle stages + // "reusable" - stage can be burnt more than once // "c" - target orbit parameters (TODO) - local num_legs is burns:length * 2. - if initial_coast = false { + local num_legs is maneuver:burns:length * 2. + if maneuver:initial_coast = false { set num_legs to num_legs - 1. } - for iter in RANGE(itmax) { + local u0 is maneuver:u0. + local du0 is maneuver:du0. + local r0 is maneuver:r0. + local v0 is maneuver:v0. + local q01 is list(u0:x, u0:y, u0:z, du0:x, du0:y, du0:z). + + for iter in RANGE(maneuver:itmax) { local q0 is list(u0:x, u0:y, u0:z, du0:x, du0:y, du0:z). - local q01 is list(u0:x, u0:y, u0:z, du0:x, du0:y, dy0:z). local x0 is list(r0:x, r0:y, r0:z, v0:x, v0:y, v0:z). local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). local qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). + local state is lex( + "mass", maneuver:mass, + "stage", 0, + "fuel", list() + ). + + for stg in maneuver:stages { + state:fuel:add(stg:fuel). + } + // Initialization of matrix of partials // z(i,j) partial of state and costate with respect to initial costate and switching times // e(i,j) partial of right end variables and switching conditions with respect to initial costate and switching times @@ -56,7 +74,7 @@ local function iterate { local uk is maneuver:mu. local prev_burn is false. for burn in maneuver:burns { - if (leg <> 0) or (initial_coast <> false) { + if (leg <> 0) or (maneuver:initial_coast <> false) { local phi is list(). local dphi is list(). for i in range(6) { @@ -69,7 +87,7 @@ local function iterate { } local start is false. - if prev_burn <> false { + if not prev_burn:istype("Boolean") { set start to prev_burn:end. } else { set start to maneuver:initial_coast. @@ -103,7 +121,7 @@ local function iterate { } } local um is sqrt(qf[0]*qf[0]+qf[1]*qf[1]+qf[2]*qf[2]). - local cbmu is burn:thrust / (mass*um). + local cbmu is maneuver:stages[state:stage]:thrust / (mass*um). for i in range(3) { set z[i+3][leg6] to -cbmu * qf[i]. } @@ -145,6 +163,9 @@ local function iterate { } set e[leg5][leg5] to 0.0. + print "Coast". + print xf. + print qf. set x0 to xf. set q0 to qf. set xf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). @@ -154,15 +175,15 @@ local function iterate { // Called to propagate burn arcs // x0(q0) - state(costate) at start of burn // xf(qf) - state(costate) at end of burn - rkg031(x0, q0, xf, qf, z, evt, hmax, maneuver, no). + rkg031(x0, q0, xf, qf, z, state, maneuver, burn, leg, num_legs, no). if leg < (num_legs-1) { local um is sqrt(qf[0]*qf[0]+qf[1]*qf[1]+qf[2]*qf[2]). set ump to um. - local cbmu is burn:thrust / (mass * um). + local cbmu is maneuver:stages[state:stage]:thrust / (mass*um). for i in range(3) { set z[i+3][leg6] to cbmu * qf[i]. } - for i in range(leg7) { + for j in range(leg7) { for k in range(3) { set e[leg6][j] to e[leg6][j] + (qf[k]/um)*z[k+6][j]. } @@ -191,12 +212,19 @@ local function iterate { set leg5 to leg + 5. set leg6 to leg + 6. set leg7 to leg + 7. + set prev_burn to burn. + + print "Burn". + print xf. + print qf. + print "mass: " + state:mass. set x0 to xf. set q0 to qf. set xf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). set qf to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). } + break. // bveval(xf, qf, z, c, e, dc). // When we have convergeence, break for i in range(6) { @@ -217,6 +245,236 @@ local function iterate { break. } } + set maneuver:u to V(q01[0], q01[1], q01[2]). + set maneuver:du to V(q01[3], q01[4], q01[5]). +} + +local function build_matrix { + parameter rows, cols. + local out is list(). + for i in range(rows) { + out:add(list()). + for j in range(cols) { + out[i]:add(0.0). + } + } + return out. +} + +local function rkg031 { + parameter x0, q0, xf, qf, z, state, maneuver, burn, leg, num_legs, no. + // This routine governs the propagation of burn arcs by calling the + // numerical integration routines. It determines the integration step + // size(h) by an estimating error after each integration step. + // x0 and q0 are state and costate at start of arc. xf and qf are state + // and costate at end of arc. z is matrix of partials. + local leg6 is leg + 6. + local hmax is maneuver:hmax. + local h is hmax. + + local yn is build_matrix(6, num_legs+7). + local ydn is build_matrix(6, num_legs+7). + local y3h is build_matrix(6, num_legs+7). + local yd3h is build_matrix(6, num_legs+7). + local ym is build_matrix(6, num_legs+7). + local ydm is build_matrix(6, num_legs+7). + local ey is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). + local eyd is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). + + for i in range(3) { + set yn[i][0] to x0[i]. + set yn[i+3][0] to q0[i]. + set ydn[i][0] to x0[i+3]. + set ydn[i+3][0] to q0[i+3]. + for j in range(1, leg6+1) { + local j1 is j-1. + set yn[i][j] to z[i][j1]. + set yn[i+3][j] to z[i+6][j1]. + set ydn[i][j] to z[i+3][j1]. + set ydn[i+3][j] to z[i+9][j]. + } + } + set tf to burn:end. + set t0 to burn:start. + set tn to t0. + until false { + if abs(h) > abs(tf-tn) { + set h to tf - tn. + } + if abs(h) > hmax { + set h to min(max(h, -hmax),hmax). + } + local stage_burn_time is state:fuel[state:stage] / maneuver:stages[state:stage]:massflow. + local do_stage is false. + if abs(h) > stage_burn_time { + set h to min(max(h, -stage_burn_time), stage_burn_time). + set do_stage to true. + } + local ho3 is h/3.0. + rkstep(yn, ydn, tn, y3h, yd3h, h, no, state, maneuver, leg). + rkstep(yn, ydn, tn, ym, ydm, ho3, false, state, maneuver, leg). + rkstep(ym, ydm, tn+ho3, ym, ydm, ho3, false, state, maneuver, leg). + rkstep(ym, ydm, tn+ho3*2.0, ym, ydm, ho3, false, state, maneuver, leg). + + set burned to maneuver:stages[state:stage]:massflow * h. + set state["fuel"][state:stage] to state:fuel[state:stage] - burned. + set state:mass to state:mass - burned. + if do_stage { + set state:mass to state:mass - maneuver:stages[state:stage]:drymass. + set state:stage to state:stage + 1. + } + + for i in range(6) { + set ey[i] to 0.0125 * (ym[i][0] - y3h[i][0]). + set eyd[i] to 0.0125 * (ydm[i][0] - yd3h[i][0]). + } + local ev2min is 1.0e-13*(ydm[0][0]*ydm[0][0] + ydm[1][0]*ydm[1][0] + ydm[2][0]*ydm[2][0]). + local evl2 is max(evt*((h/(tf-t0)) ^ 2.0), ev2min). + local r_ is (eyd[0]*eyd[0] + eyd[1]*eyd[1] + eyd[2]*eyd[2])/evl2. + for i in range(6) { + set yn[i][0] to ym[i][0] + ey[i]. + set ydn[i][0] to ydm[i][0] + eyd[i]. + for j in range(1, leg6+1) { + set yn[i][j] to y3h[i][j]. + set ydn[i][j] to yd3h[i][j]. + } + } + + if abs(h) >= abs(tf-tn) { + if not maneuver:stages[state:stage]:reusable { + set state:mass to state:mass - (state:fuel[state:stage] + maneuver:stages[state:stage]:dry_mass). + set state:stage to state:stage + 1. + } + break. + } + + set tn to tn + h. + if r_ < 0.04 { + set r_ to 0.04. + } + //set h to h / (r_ ^ 0.125). + } + // Calculation of partial of xf and qf with respect to final time (tf) + yddrhs(yn, yd3h, 1.0, 0.0, state, maneuver, leg, false). + local l6 is num_legs+5. + for i in range(3) { + set z[i][l6] to ydn[i][0]. + set z[i+3][l6] to yd3h[i][0]. + set z[i+6][l6] to ydn[i][0]. + set z[i+9][l6] to yd3h[i][0]. + } + for i in range(3) { + set xf[i] to yn[i][0]. + set xf[i+3] to ydn[i][0]. + set qf[i] to yn[i+3][0]. + set qf[i+3] to ydn[i+3][0]. + for j in range(1,leg6+1) { + local j1 is j-1. + set z[i][j1] to yn[i][j]. + set z[i+3][j1] to ydn[i][j]. + set z[i+6][j1] to yn[i+3][j]. + set z[i+9][j1] to ydn[i+3][j]. + } + } +} + +local function rkstep { + parameter yn, ydn, tn, yn1, ydn1, h, n, state, maneuver, leg. + local h2 is h / 2.0. + local jmax is 1. + if n { + set jmax to leg+7. + } + local d1 is build_matrix(6, jmax). + local d2 is build_matrix(6, jmax). + local d3 is build_matrix(6, jmax). + local y is build_matrix(6, jmax). + yddrhs(yn, d1, h, 0.0, state, maneuver, leg, n). + for j in range(jmax) { + for i in range(6) { + set y[i][j] to yn[i][j] + h2*(ydn[i][j] + 0.25*d1[i][j]). + } + } + yddrhs(y, d2, h, h2, state, maneuver, leg, n). + for j in range(jmax) { + for i in range(6) { + set y[i][j] to yn[i][j] + h*(ydn[i][j] + 0.5*d2[i][j]). + } + } + yddrhs(y, d3, h, h, state, maneuver, leg, n). + for j in range(jmax) { + for i in range(6) { + set yn1[i][j] to yn[i][j] + h*(ydn[i][j] + (d1[i][j] + 2.0*d2[i][j])/6.0). + set ydn1[i][j] to ydn[i][j] + (d1[i][j] + 4.0*d2[i][j] + d3[i][j])/6.0. + } + } +} + +local function yddrhs { + parameter y, ydd, h, t, state, maneuver, leg, n. + local leg7 is leg+7. + local uk is maneuver:mu. + + local r_ is list(y[0][0], y[1][0], y[2][0]). + local u is list(y[3][0], y[4][0], y[5][0]). + local am1 is state:mass - t * maneuver:stages[state:stage]:massflow. + local r2 is 1.0 / (r_[0]*r_[0] + r_[1]*r_[1] + r_[2]*r_[2]). + local u2 is 1.0 / (u[0]*u[0] + u[1]*u[1] + u[2]*u[2]). + local um is sqrt(u2). + local ru is r_[0]*u[0] + r_[1]*u[1] + r_[2]*u[2]. + local alpha is -h*uk*r2*sqrt(r2). + local beta is h*um*maneuver:stages[state:stage]:thrust / am1. + local gamma is -3.0*alpha*r2*ru. + for i in range(3) { + set ydd[i][0] to r_[i]*alpha + u[i]*beta. + set ydd[i+3][0] to r_[i]*gamma + u[i]*alpha. + } + + if not n { + return. + } + // Compute additional quantities common to many comopnents of wdd + local delta is -3.0*alpha*r2. + local epsil is -beta*u2. + local zeta is -5.0*gamma*r2. + // compute the matrix b needed in the matrix equation wdd=b*w + local b is build_matrix(6, 6). + for j in range(3) { + local rrj is delta*r_[j]. + local ruj is epsil*u[j]. + local urj is zeta*r_[j] + delta*u[j]. + for i in range(3) { + if i <> j { + set b[i][j] to r_[i]*rrj. + set b[i][j+3] to u[i]*ruj. + set b[i+3][j] to r_[i]*urj+u[i]*rrj. + } else { + set b[i][j] to r_[i]*rrj + alpha. + set b[i][j+3] to u[i]*ruj+beta. + set b[i+3][j] to r_[i]*urj + u[i]*rrj + gamma. + } + set b[i+3][j+3] to b[i][j]. + } + } + // perform the matrix multiplication b*w to get wdd + for i in range(6) { + for j in range(leg7) { + local sum is 0.0. + for k in range(6) { + set sum to sum + b[i][k]*y[k][j]. + } + set ydd[i][j] to sum. + } + } + if leg <> 0 { + local prddm is beta / am1. + for j in range(7, leg7) { + local rddmb is prddm*(0.0 - maneuver:stages[state:stage]:thrust). + for i in range(3) { + set ydd[i][j] to ydd[i][j] + rddmb*u[i]. + } + } + } } local function coast { @@ -448,26 +706,34 @@ local function amult { } } -local function test_coast { - local x0 is list(4551.3085900, 4719.8398400, 25.0576324, 5.5990610, -5.4170895, -0.0118389). - local q0 is list(0.4601788, -0.8868545, 0.0415273, -0.0004394, -0.0010504, -0.0004081). - local uk is 398601.5. - local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). - local qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0). - local t is 2047.6868 - 934. - local phi is list(). - local dphi is list(). - for i in range(6) { - phi:add(list()). - dphi:add(list()). - for j in range(6) { - phi[i]:add(0.0). - dphi[i]:add(0.0). - } - } - coast(x0, q0, xf, qf, phi, dphi, uk, t, true). - print xf. - print qf. +local function test_switch { + local maneuver is lex( + "mass", 12644651.0, + "mu", 398601.5, + "hmax", 100.0, + "itmax", 10, + "r0", V(4551.3085900, 4719.8398400, 25.0576324), + "v0", V(5.5990610, -5.4170895, -0.0118389), + "u0", V(0.4601788, -0.8868545, 0.0415273), + "du0", V(-0.0004394, -0.0010504, -0.0004081), + "burns", list( + lex("start", 2047.6868, "end", 2302.8677), + lex("start", 21032.055, "end", 21150.852) + ), + "initial_coast", 934.0, + "stages", list( + lex( + "thrust", 92986.438, + "massflow", 22384.406, + "fuel", 10000000.0, + "dry_mass", 0.0, + "reusable", true + ) + ) + ). + iterate(maneuver, true). + print maneuver:u0. + print maneuver:du0. } -test_coast(). \ No newline at end of file +test_switch(). \ No newline at end of file