473 lines
16 KiB
Plaintext
473 lines
16 KiB
Plaintext
local function iterate {
|
|
parameter maneuver, no.
|
|
// `maneuver` is a lexicon of:
|
|
// "mass" - vehicle mass at start of maneuver
|
|
// "mu" - gravitational constant
|
|
// "hmax" - maximum step size
|
|
// "itmax" - maximum number of iterations
|
|
// "r0" - initial position
|
|
// "v0" - initial velocity
|
|
// "u0" - initial heading
|
|
// "du0" - initial change in heading
|
|
// "burns" - list of lexicon
|
|
// "start" - burn start time
|
|
// "end" - burn end time
|
|
// "initial_coast" - either start time of coast, or `false` if none
|
|
// "stages" - list of lexicon
|
|
// "thrust" - engine thrust
|
|
// "massflow" - fuel consumption of engine
|
|
// "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
|
|
// "c" - target orbit parameters (TODO)
|
|
|
|
local num_legs is burns:length * 2.
|
|
if initial_coast = false {
|
|
set num_legs to num_legs - 1.
|
|
}
|
|
|
|
for iter in RANGE(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).
|
|
|
|
// 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
|
|
local z is list().
|
|
local e is list().
|
|
for i in RANGE(12) {
|
|
z:add(list()).
|
|
e:add(list()).
|
|
for j in RANGE(6 + num_legs) {
|
|
z[i]:add(0.0).
|
|
e[i]:add(0.0).
|
|
}
|
|
e[i]:add(0.0).
|
|
}
|
|
local leg is 0.
|
|
local leg5 is 5.
|
|
local leg6 is 6.
|
|
local leg7 is 7.
|
|
local l7 is num_legs+6.
|
|
local ump is 0.0.
|
|
local uk is maneuver:mu.
|
|
local prev_burn is false.
|
|
for burn in maneuver:burns {
|
|
if (leg <> 0) or (initial_coast <> false) {
|
|
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).
|
|
}
|
|
}
|
|
|
|
local start is false.
|
|
if prev_burn <> false {
|
|
set start to prev_burn:end.
|
|
} else {
|
|
set start to maneuver:initial_coast.
|
|
}
|
|
local end is burn:start.
|
|
local t is end - start.
|
|
// phi(i,j) partial of state at end of coast with respect to state at start of coast
|
|
// dphi(i,j) partial of costate at end of coast with respect to state at start of coast
|
|
coast (x0, q0, xf, qf, phi, dphi, uk, t, no).
|
|
|
|
local dz is list().
|
|
for i in range(12) {
|
|
dz:add(list()).
|
|
for j in RANGE(leg6) {
|
|
dz[i]:add(0.0).
|
|
}
|
|
}
|
|
for i in range(6) {
|
|
local i6 is i + 6.
|
|
for j in range(leg6) {
|
|
for k in range(6) {
|
|
set dz[i][j] to dz[i][j] + phi[i][k] * z[k][j].
|
|
set dz[i6][j] to dz[i6][j] + phi[i][k] * z[k+6][j] + dphi[i][k] * z[k][j].
|
|
}
|
|
}
|
|
}
|
|
// Update matrix of partials Z
|
|
for i in range(12) {
|
|
for j in range(leg6) {
|
|
set z[i][j] to dz[i][j].
|
|
}
|
|
}
|
|
local um is sqrt(qf[0]*qf[0]+qf[1]*qf[1]+qf[2]*qf[2]).
|
|
local cbmu is burn:thrust / (mass*um).
|
|
for i in range(3) {
|
|
set z[i+3][leg6] to -cbmu * qf[i].
|
|
}
|
|
if leg <> 0 {
|
|
// Calculation of switching condition and corresponding partial
|
|
set e[leg5][l7] to ump - um.
|
|
for j in range(leg5+1) {
|
|
set e[leg5][j] to -e[leg5][j].
|
|
for k in range(3) {
|
|
set e[leg5][j] to e[leg5][j] + (qf[k]/um)*z[k+6][j].
|
|
}
|
|
}
|
|
}
|
|
set leg to leg + 1.
|
|
set leg5 to leg + 5.
|
|
set leg6 to leg + 6.
|
|
set leg7 to leg + 7.
|
|
|
|
// Calculation of transversality condition and corresponding partial with respect to initial costate and switching times
|
|
local r2 is xf[0]*xf[0] + xf[1]*xf[1] + xf[2]*xf[2].
|
|
local rs is xf[0]*qf[0] + xf[1]*qf[1] + xf[2]*qf[2].
|
|
local c3 is -uk / (r2 * sqrt(r2)).
|
|
local c4 is -3.0 * c3 * rs / r2.
|
|
set e[leg5][l7] to xf[3]*qf[3]+xf[4]*qf[4]+xf[5]*qf[5] - c3*rs - e[leg5][l7].
|
|
set dummy to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
|
|
for i in range(3) {
|
|
set dummy[i] to -c3*qf[i]-c4*xf[i].
|
|
set dummy[i+3] to qf[i+3].
|
|
set dummy[i+6] to - c3 * xf[i].
|
|
set dummy[i+9] to xf[i+3].
|
|
}
|
|
for j in range(leg7) {
|
|
for k in range(12) {
|
|
set e[leg5][j] to -dummy[k]*z[k][j] + e[leg5][j].
|
|
}
|
|
}
|
|
for j in range(leg7) {
|
|
set e[leg5][j] to -e[leg5][j].
|
|
}
|
|
set e[leg5][leg5] to 0.0.
|
|
|
|
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).
|
|
}
|
|
// burn
|
|
// 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).
|
|
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).
|
|
for i in range(3) {
|
|
set z[i+3][leg6] to cbmu * qf[i].
|
|
}
|
|
for i in range(leg7) {
|
|
for k in range(3) {
|
|
set e[leg6][j] to e[leg6][j] + (qf[k]/um)*z[k+6][j].
|
|
}
|
|
}
|
|
}
|
|
if leg <> 0 {
|
|
local r2 is xf[0]*xf[0] + xf[1]*xf[1] + xf[2]*xf[2].
|
|
local rs is xf[0]*qf[0] + xf[1]*qf[1] + xf[2]*qf[2].
|
|
local c3 is -uk / (r2 * sqrt(r2)).
|
|
local c4 is -3.0 * c3 * rs / r2.
|
|
set e[leg5][l7] to xf[3]*qf[3]+xf[4]*qf[4]+xf[5]*qf[5] - c3*rs - e[leg5][l7].
|
|
set dummy to list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
|
|
for i in range(3) {
|
|
set dummy[i] to -c3*qf[i]-c4*xf[i].
|
|
set dummy[i+3] to qf[i+3].
|
|
set dummy[i+6] to - c3 * xf[i].
|
|
set dummy[i+9] to xf[i+3].
|
|
}
|
|
for j in range(leg7) {
|
|
for k in range(12) {
|
|
set e[leg5][j] to -dummy[k]*z[k][j] + e[leg5][j].
|
|
}
|
|
}
|
|
}
|
|
set leg to leg + 1.
|
|
set leg5 to leg + 5.
|
|
set leg6 to leg + 6.
|
|
set leg7 to leg + 7.
|
|
set prev_burn to burn.
|
|
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).
|
|
}
|
|
// bveval(xf, qf, z, c, e, dc).
|
|
// When we have convergeence, break
|
|
for i in range(6) {
|
|
local i6 is i+6.
|
|
set e[i][l7] to dc[i].
|
|
set dc[i+6] to e[i+7][l7].
|
|
set e[leg6][i] to q01[i].
|
|
}
|
|
if no {
|
|
// called to solve simultaneous equations and determine new initial
|
|
// costate and switching times. Q01- initial costate
|
|
// adjust(q01, e, z, du, dudt, legmax, atp).
|
|
|
|
if du <0.0001 or dudt < 0.001 {
|
|
set no to true.
|
|
}
|
|
} else {
|
|
break.
|
|
}
|
|
}
|
|
}
|
|
|
|
local function coast {
|
|
parameter x0, q0, xf, qf, phi, dphi, uk, t, no.
|
|
local jump is false.
|
|
local rm0 is sqrt(x0[0]*x0[0] + x0[1]*x0[1] + x0[2]*x0[2]).
|
|
local drm0 is (x0[0]*q0[0] + x0[1]*q0[1] + x0[2]*q0[2])/rm0.
|
|
local sig0 is x0[0]*x0[3] + x0[1]*x0[4] + x0[2]*q0[5].
|
|
local dsig0 is x0[3]*q0[0] + x0[4]*q0[1] + x0[5]*q0[2] + x0[0]*q0[3] + x0[1]*q0[4] + x0[2]*q0[5].
|
|
local alpha is x0[3]*x0[3] + x0[4]*x0[4] + x0[5]*x0[5] - 2.0 * uk / rm0.
|
|
local h0 is list(
|
|
x0[1]*x0[5] - x0[2]*x0[4],
|
|
x0[2]*x0[3] - x0[0]*x0[5],
|
|
x0[0]*x0[4] - x0[1]*x0[3]
|
|
).
|
|
local p0 is (h0[0]*h0[0] + h0[1]*h0[1] + h0[2]*h0[2])/uk.
|
|
local psy is t/p0.
|
|
local alpsq is sqrt(-alpha). // This probably blows up for non-eliptical orbits?
|
|
local s0 is 0.0.
|
|
local s1 is 0.0.
|
|
local s2 is 0.0.
|
|
local s3 is 0.0.
|
|
local ft is t.
|
|
local rm is rm0.
|
|
until false {
|
|
local alpsy is psy * alpsq.
|
|
set s0 to cos(alpsy*CONSTANT:RadTodeg).
|
|
set s1 to sin(alpsy*CONSTANT:RadToDeg)/alpsq.
|
|
set s2 to (s0 - 1.0) / alpha.
|
|
set s3 to (s1 - psy) / alpha.
|
|
set ft to rm0 * s1 + sig0 * s2 + uk * s3.
|
|
set rm to rm0*s0 + sig0*s1 + uk*s2.
|
|
if jump {
|
|
break.
|
|
}
|
|
set psy to psy + (t-ft) / rm.
|
|
|
|
if abs(t-ft) < abs(t) * 0.00001 {
|
|
set jump to true.
|
|
}
|
|
}
|
|
print psy.
|
|
print alpha.
|
|
print ft.
|
|
local fm1 is -uk * s2 / rm0.
|
|
local f is 1.0 + fm1.
|
|
local fd is -uk * s1 / (rm * rm0).
|
|
local g is ft - uk * s3.
|
|
local gdm1 is -uk * s2 / rm.
|
|
local gd is 1.0 + gdm1.
|
|
local ukr3 is uk / (rm * rm * rm).
|
|
local ukr03 is uk / (rm0 * rm0 * rm0).
|
|
local dalph is 2.0 * (x0[3]*q0[3] + x0[4]*q0[4] + x0[5]*q0[5] + ukr03 * (x0[0]*q0[0] + x0[1]*q0[1] + x0[2]*q0[2])).
|
|
local dapa is dalph/alpha.
|
|
local dapa2 is dapa/alpha.
|
|
local dpsy is -(drm0*s1 + dsig0*s2 + rm0*(psy*s0-s1)*dapa*0.5 + sig0*(psy*s1*0.5 - s2)*dapa + uk*(psy - 1.5*s1 + psy*s0*0.5)*dapa2)/rm.
|
|
local ds0 is (alpha*dpsy + 0.5*psy*dalph)*s1.
|
|
local ds1 is s0*dpsy + (psy*s0 - s1)*dapa*0.5.
|
|
local ds2 is s1*dpsy + (0.5*psy*s1 - s2)*dapa.
|
|
local ds3 is s2*dpsy + (psy - 1.5*s1 + 0.5*psy*s0)*dapa2.
|
|
local s4 is (s2 - psy*psy*0.5)/alpha.
|
|
local ds4 is s3*dpsy + (psy*psy*0.5 - 2.0*s2 + 0.5*psy*s1)*dapa2.
|
|
local s5 is (s3 - psy*psy*psy/6.0)/alpha.
|
|
local ds5 is s4*dpsy+(psy*psy*psy/6.0 + (2.0*psy - 2.5*s1 + 0.5*psy*20)/alpha)*dapa2.
|
|
local u is s2*ft + uk*(psy*s4 - 3.0*s5).
|
|
local du is ds2*ft + uk*(dpsy*s4 + psy*ds4 - 3.0*ds5).
|
|
local drm is 20*drm0 + ds0*rm0 + s1*dsig0 + ds1*sig0 + uk*ds2.
|
|
local df is (-uk*ds2 - fm1*drm0)/rm0.
|
|
local dg is -uk*ds3.
|
|
local dgd is (-uk*ds2 - gdm1*drm)/rm.
|
|
local r01 is rm0*rm.
|
|
local dr01 is rm*drm0 + drm*rm0.
|
|
local dfd is (-uk*ds1 - fd*dr01)/r01.
|
|
for i in range(3) {
|
|
set xf[i] to x0[i]*f + x0[i+3]*g.
|
|
set xf[i+3] to x0[i]*fd + x0[i+3]*gd.
|
|
set qf[i] to x0[i]*df + x0[i+3]*dg + q0[i]*f + q0[i+3]*g.
|
|
set qf[i+3] to x0[i]*dfd + x0[i+3]*dgd + q0[i]*fd + q0[i+3]*gd..
|
|
}
|
|
|
|
if no = false {
|
|
return.
|
|
}
|
|
// calculation of partials
|
|
set dukr3 to -3.0*ukr3*drm/rm.
|
|
set dur03 to -3.0*ukr03*drm0/rm0.
|
|
set s1r0 to s1/rm0.
|
|
set ds1r0 to (ds1-s1r0*drm0)/rm0.
|
|
set s1r to s1/rm.
|
|
set ds1r to (ds1-s1r*drm)/rm.
|
|
set r02 to 1.0/(rm0*rm0).
|
|
set r2 to 1.0/(rm*rm).
|
|
set uuk03 to -u*ukr03.
|
|
set duuk3 to -du*ukr03-u*dur03.
|
|
|
|
local ann is list(
|
|
list(
|
|
-fd*s1r0 - fm1*r02,
|
|
-fd*s2
|
|
),
|
|
list(
|
|
fm1*s1r0 + uuk03,
|
|
fm1*s2
|
|
)
|
|
).
|
|
local dumm1 is ann[0][0].
|
|
local dan is list(
|
|
list(
|
|
-fd*ds1r0 - dfd*s1r0 + ukr03*ds2 + dur03*s2,
|
|
-dfd*s2 - fd*ds2
|
|
),
|
|
list(
|
|
fm1*ds1r0 + df*s1r0 + duuk3,
|
|
fm1*ds2 + df*s2
|
|
)
|
|
).
|
|
local dummy is dan[0][0].
|
|
local bnn is list().
|
|
local dbnn is list().
|
|
for i in range(3) {
|
|
bnn:add(list()).
|
|
dbnn:add(list()).
|
|
for j in range(3) {
|
|
bnn[i]:add(0.0).
|
|
dbnn[i]:add(0.0).
|
|
}
|
|
}
|
|
|
|
amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
|
|
for i in range(3) {
|
|
for j in range(3) {
|
|
set dphi[i][j] to dbnn[i][j].
|
|
set phi[i][j] to bnn[i][j].
|
|
}
|
|
}
|
|
for i in range(3) {
|
|
set dphi[i][i] to dphi[i][i] + df.
|
|
set phi[i][i] to phi[i][i] + f.
|
|
}
|
|
|
|
set ann[0][0] to ann[0][1].
|
|
set ann[1][0] to ann[1][1].
|
|
set ann[0][1] to -gdm1*ds2 - dgd*s2.
|
|
set ann[1][1] to g*s2 - u.
|
|
set dan[0][0] to dan[0][1].
|
|
set dan[1][0] to dan[1][1].
|
|
set dan[0][1] to -gdm1*ds2 - dgd*s2.
|
|
set dan[1][1] to -du + dg*s2 + g*ds2.
|
|
|
|
amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
|
|
for i in range(3) {
|
|
for j in range(3) {
|
|
local j3 is j+3.
|
|
set dphi[i][j3] to dbnn[i][j].
|
|
set phi[i][j3] to bnn[i][j].
|
|
}
|
|
}
|
|
for i in range(3) {
|
|
local i3 is i + 3.
|
|
set dphi[i][i3] to dphi[i][i3] + dg.
|
|
set phi[i][i3] to phi[i][i3] + g.
|
|
}
|
|
|
|
set ann[1][0] to -ann[0][0].
|
|
set ann[1][1] to -ann[0][1].
|
|
set ann[0][0] to -fd*s1r - gdm1*r2.
|
|
set ann[0][1] to u*ukr3 - gdm1*s1r.
|
|
set dan[1][0] to -dan[0][0].
|
|
set dan[1][1] to -dan[0][1].
|
|
set dan[0][0] to -dfd*s1r -fd*ds1r + ukr3*ds2 + dukr3*s2.
|
|
set dan[0][1] to -gdm1*ds1r - dgd*s1r + du*ukr3 + u*dukr3.
|
|
|
|
amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
|
|
for i in range(3) {
|
|
local i3 is i+3.
|
|
for j in range(3) {
|
|
local j3 is j+3.
|
|
set dphi[i3][j3] to dbnn[i][j].
|
|
set phi[i3][j3] to bnn[i][j].
|
|
}
|
|
}
|
|
for i in range(3) {
|
|
local i3 is i + 3.
|
|
set dphi[i3][i3] to dphi[i][i3] + dgd.
|
|
set phi[i3][i3] to phi[i][i3] + gd.
|
|
}
|
|
|
|
set ann[0][1] to ann[0][0].
|
|
set ann[1][1] to ann[1][0].
|
|
set ann[1][0] to -dumm1.
|
|
set ann[0][0] to -fd*(s0/r01 + r2 + r02) - uuk03*ukr3.
|
|
set dan[0][1] to dan[0][0].
|
|
set dan[1][1] to dan[1][0].
|
|
set dan[1][0] to -dummy.
|
|
set dan[0][0] to -uuk03*dukr3 - duuk3*ukr3 - dfd*(s0/r01 + r2 + r02) - fd*((ds0 - s0*dr01/r01)/r01 - 2.0*(r2*drm/rm + r02*drm0/rm0)).
|
|
|
|
amult(ann, dan, bnn, dbnn, x0, q0, xf, qf).
|
|
for i in range(3) {
|
|
local i3 is i+3.
|
|
for j in range(3) {
|
|
set dphi[i3][j] to dbnn[i][j].
|
|
set phi[i3][j] to bnn[i][j].
|
|
}
|
|
}
|
|
for i in range(3) {
|
|
local i3 is i + 3.
|
|
set dphi[i3][i] to dphi[i][i3] + dfd.
|
|
set phi[i3][i] to phi[i][i3] + fd.
|
|
}
|
|
}
|
|
|
|
local function amult {
|
|
parameter ann, dan, bnn, dbnn, x0, q0, xf, qf.
|
|
local da is list().
|
|
local a is list().
|
|
for i in range(3) {
|
|
da:add(list()).
|
|
a:add(list()).
|
|
for j in range(2) {
|
|
da[i]:add(qf[i]*ann[0][j] + qf[i+3]*ann[1][j] + xf[i]*dan[0][j] + xf[i+3]*dan[1][j]).
|
|
a[i]:add(ann[0][j]*xf[i] + ann[1][j]*xf[i+3]).
|
|
}
|
|
}
|
|
for i in range(3) {
|
|
for j in range(3) {
|
|
set dbnn[i][j] to a[i][0]*q0[j] + a[i][1]*q0[j+3] + da[i][0]*x0[j] + da[i][1]*x0[j+3].
|
|
set bnn[i][j] to a[i][0]*x0[j] + a[i][1]*x0[j+3].
|
|
}
|
|
}
|
|
}
|
|
|
|
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.
|
|
}
|
|
|
|
test_coast(). |