Add burn integration

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.
+    local num_legs is maneuver:burns:length * 2.
-    if initial_coast = false {
+    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 function test_switch {
-    local x0 is list(4551.3085900, 4719.8398400, 25.0576324, 5.5990610, -5.4170895, -0.0118389).
+    local maneuver is lex(
-    local q0 is list(0.4601788, -0.8868545, 0.0415273, -0.0004394, -0.0010504, -0.0004081).
+            "mass", 12644651.0,
-    local uk is 398601.5.
+                    "mu", 398601.5,
-    local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
+                    "hmax", 100.0,
-    local qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
+                    "itmax", 10,
-    local t is 2047.6868 - 934.
+                    "r0", V(4551.3085900, 4719.8398400, 25.0576324),
-    local phi is list().
+                    "v0", V(5.5990610, -5.4170895, -0.0118389),
-    local dphi is list().
+                    "u0", V(0.4601788, -0.8868545, 0.0415273),
-    for i in range(6) {
+                    "du0", V(-0.0004394, -0.0010504, -0.0004081),
-        phi:add(list()).
+                    "burns", list(
-        dphi:add(list()).
+                    lex("start", 2047.6868, "end", 2302.8677),
-        for j in range(6) {
+                            lex("start", 21032.055, "end", 21150.852)
-            phi[i]:add(0.0).
+                    ),
-            dphi[i]:add(0.0).
+                    "initial_coast", 934.0,
-        }
+                    "stages", list(
-    }
+                    lex(
-    coast(x0, q0, xf, qf, phi, dphi, uk, t, true).
+                            "thrust", 92986.438,
-    print xf.
+                                    "massflow", 22384.406,
-    print qf.
+                                    "fuel", 10000000.0,
+                                    "dry_mass", 0.0,
+                                    "reusable", true
+                            )
+                    )
+            ).
+    iterate(maneuver, true).
+    print maneuver:u0.
+    print maneuver:du0.
 }
 
-test_coast().
+test_switch().