259 lines
9.8 KiB
Plaintext
259 lines
9.8 KiB
Plaintext
local function iterate {
|
|
parameter maneuver.
|
|
// `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, y0:z, du0:x, du0:y, du0: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
|
|
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.
|
|
}
|
|
// When we have convergeence, 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
|
|
}
|
|
|
|
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.
|
|
coast(x0, q0, xf, qf, list(), list(), uk, t, false).
|
|
print xf.
|
|
print qf.
|
|
}
|
|
|
|
test_coast(). |