more code
This commit is contained in:
220
lib/switch.ks
220
lib/switch.ks
@@ -1,5 +1,5 @@
|
|||||||
local function iterate {
|
local function iterate {
|
||||||
parameter maneuver.
|
parameter maneuver, no.
|
||||||
// `maneuver` is a lexicon of:
|
// `maneuver` is a lexicon of:
|
||||||
// "mass" - vehicle mass at start of maneuver
|
// "mass" - vehicle mass at start of maneuver
|
||||||
// "mu" - gravitational constant
|
// "mu" - gravitational constant
|
||||||
@@ -27,7 +27,8 @@ local function iterate {
|
|||||||
}
|
}
|
||||||
|
|
||||||
for iter in RANGE(itmax) {
|
for iter in RANGE(itmax) {
|
||||||
local q0 is list(u0:x, u0:y, y0:z, du0:x, du0:y, du0:z).
|
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 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 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 qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
|
||||||
@@ -150,13 +151,71 @@ local function iterate {
|
|||||||
set qf 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
|
// 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 leg to leg + 1.
|
||||||
set leg5 to leg + 5.
|
set leg5 to leg + 5.
|
||||||
set leg6 to leg + 6.
|
set leg6 to leg + 6.
|
||||||
set leg7 to leg + 7.
|
set leg7 to leg + 7.
|
||||||
set prev_burn to burn.
|
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
|
// 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.
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -242,6 +301,151 @@ local function coast {
|
|||||||
return.
|
return.
|
||||||
}
|
}
|
||||||
// calculation of partials
|
// 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 function test_coast {
|
||||||
@@ -251,7 +455,17 @@ local function test_coast {
|
|||||||
local xf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
|
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 qf is list(0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
|
||||||
local t is 2047.6868 - 934.
|
local t is 2047.6868 - 934.
|
||||||
coast(x0, q0, xf, qf, list(), list(), uk, t, 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).
|
||||||
|
}
|
||||||
|
}
|
||||||
|
coast(x0, q0, xf, qf, phi, dphi, uk, t, true).
|
||||||
print xf.
|
print xf.
|
||||||
print qf.
|
print qf.
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user