branan/kos-scripts
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add burn integration

Browse Source
This commit is contained in:
Branan Riley
2025-10-30 09:44:08 -07:00
parent a9a7677ca8
commit e5c23740f9
1 changed files with 297 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

328
lib/switch.ks
View File

@@ -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().
test_switch().