739 lines
25 KiB
Plaintext
739 lines
25 KiB
Plaintext
local evt is 1.0e-8.
|
|
|
|
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
|
|
// "reusable" - stage can be burnt more than once
|
|
// "c" - target orbit parameters (TODO)
|
|
|
|
local num_legs is maneuver:burns:length * 2.
|
|
if maneuver:initial_coast = false {
|
|
set num_legs to num_legs - 1.
|
|
}
|
|
|
|
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 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
|
|
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 (maneuver: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 not prev_burn:istype("Boolean") {
|
|
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 maneuver:stages[state:stage]: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.
|
|
|
|
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).
|
|
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, 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 maneuver:stages[state:stage]:thrust / (mass*um).
|
|
for i in range(3) {
|
|
set z[i+3][leg6] to cbmu * qf[i].
|
|
}
|
|
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].
|
|
}
|
|
}
|
|
}
|
|
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.
|
|
|
|
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) {
|
|
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.
|
|
}
|
|
}
|
|
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 {
|
|
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_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_switch(). |