I'm working on my own PEG implementation in KOS as a side project. I've got it sort of working, I'm having issues with the delta v - it's off by a factor of about 5. It starts decreasing towards zero, then around 30 seconds before cutoff, jumps randomly by huge amounts, changing sign. As far as I can tell I've implemented the algorithm just as described in the NASA paper / orbiter notes. If I comment out the derivative terms it basically works. Anyone here want to take a peek at my script and see if anything jumps out?
function GuidanceLoop
{
SetOldTime(GetTimer()).
SetTimer(time:seconds()).
local T is GetT().
local r_v is -ship:body:position.
local r is r_v:mag.
//solve for guidance parameters A and B
local b0 is -ve() * ln(1 - min(0.9999, T / GetTau())).
local b1 is b0 * GetTau() - ve() * t.
local c0 is b0 * t - b1.
local c1 is c0 * GetTau() - ve() * t^2 / 2.
local M_B_0 is r_d_t - verticalspeed.
local M_B_1 is r_t - r - verticalspeed * T.
//update steering parameters
if T > 10
{
SetB(((M_B_1 / c0) - (M_B_0 / b0)) / ((c1 / c0) - (b1 / b0))).
SetA((M_B_0 / b0) - GetB() * b1 / b0).
SetOldA(GetA() + delta_t * GetB()).
SetOldB(GetB()).
}
SetOldT(T - delta_t).
//update state vectors
local h_v is vcrs(r_v, ship:velocity:orbit).
local theta_unit is vcrs(h_v:normalized, r_v:normalized).
local delta_h is h_t - h_v:mag.
local r_mean is (r_t + r) / 2.
//performance
local omega is vdot(ship:velocity:orbit, theta_unit) / r.
local C_t is C(r_t, omega_t, GetOldT(), GetTau()).
SetTau(tau()).
if T > 10
{
SetC(C(r, omega, 0, GetTau())).
}
//calculate delta v
local f_r is GetOldA() + GetC().
local f_r_d is GetOldB() + (C_t - f_r) / GetOldT().
local f_theta is 1 - f_r^2 / 2.
local f_theta_d is -f_r * f_r_d.
local f_theta_d2 is - f_theta_d^2 / 2.
//local delta_v is (delta_h / r_mean) + ve()* T * (f_theta_d + f_theta_d2) * GetTau() + (f_theta_d2 * ve() * T^2 / 2 ).
//set delta_v to delta_v / (f_theta + f_theta_d * GetTau() + f_theta_d2 * GetTau()^2).
local delta_v is (delta_h / r_mean) / f_theta.
SetT(GetTau() * (1 - constant:e^(-delta_v / ve()))).
}