From f389febcc88528da863779c25140fc6dc8759bac Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Tue, 16 Jan 2018 16:22:58 -0700 Subject: [PATCH] Fixed Ruggiero to work with small differences in initial and final OEs Until now it would lead to a NaN error, so it was prevented. however, that causes Case A of Petropoulos to fail because the eccentricity was never correct on with Ruggerio. In fact, the sma correction led to a change in ecc, but because the initial and target ecc was equal, no correction could mathematically be applied without causing a NaN. --- mission_test.go | 9 ++++++--- orbit.go | 5 +++++ prop.go | 53 ++++++++++++++++++++++++++----------------------- waypoints.go | 2 +- 4 files changed, 40 insertions(+), 29 deletions(-) diff --git a/mission_test.go b/mission_test.go index e8626f3..ced584f 100644 --- a/mission_test.go +++ b/mission_test.go @@ -554,15 +554,18 @@ func TestMultiCorrectOE(t *testing.T) { } func TestPetropoulosCaseA(t *testing.T) { - t.Log("Case A fails with Ruggiero: stops although the eccenticity is not good)") - for _, meth := range []ControlLawType{Naasz} { + t.Log("Case A duration with Ruggiero is incorrect: it should be 14.6 days, but takes 31.19") + distanceε := 1.0 + for _, meth := range []ControlLawType{Ruggiero, Naasz} { oInit := NewOrbitFromOE(Earth.Radius+1000, 0.01, 0.05, 0, 0, 1, Earth) oTarget := NewOrbitFromOE(42164, 0.01, 0.05, 0, 0, 1, Earth) eps := NewUnlimitedEPS() EPThrusters := []EPThruster{NewGenericEP(1, 3100)} dryMass := 1.0 fuelMass := 299.0 - sc := NewSpacecraft("Petro", dryMass, fuelMass, eps, EPThrusters, false, []*Cargo{}, []Waypoint{NewOrbitTarget(*oTarget, nil, meth, OptiΔaCL, OptiΔeCL)}) + orbitTgt := NewOrbitTarget(*oTarget, nil, meth, OptiΔaCL, OptiΔeCL) + orbitTgt.SetEpsilons(distanceε, eccentricityε, angleε) + sc := NewSpacecraft("Petro", dryMass, fuelMass, eps, EPThrusters, false, []*Cargo{}, []Waypoint{orbitTgt}) start := time.Date(2017, 1, 1, 0, 0, 0, 0, time.UTC) // With eta=0, the duration is 14.600 days. //end := start.Add(time.Duration(15*24) * time.Hour) diff --git a/orbit.go b/orbit.go index bd9b1b7..cb797d5 100644 --- a/orbit.go +++ b/orbit.go @@ -271,6 +271,11 @@ func (o Orbit) epsilons() (float64, float64, float64) { // Equals returns whether two orbits are identical with free true anomaly. // Use StrictlyEquals to also check true anomaly. func (o Orbit) Equals(o1 Orbit) (bool, error) { + return o.EqualsWithin(o1, distanceε, eccentricityε, angleε) +} + +// EqualsWithin returns whether two orbits are identical with free true anomaly and within provided bounds. +func (o Orbit) EqualsWithin(o1 Orbit, distanceε, eccentricityε, angleε float64) (bool, error) { if !o.Origin.Equals(o1.Origin) { return false, errors.New("different origin") } diff --git a/prop.go b/prop.go index 835a733..bdf4d92 100644 --- a/prop.go +++ b/prop.go @@ -253,7 +253,7 @@ type OptimalΔOrbit struct { // local copy of the OEs of the inital and target orbits oInita, oInite, oIniti, oInitΩ, oInitω, oInitν float64 oTgta, oTgte, oTgti, oTgtΩ, oTgtω, oTgtν float64 - distanceε, eccentricityε, angleε float64 + Distanceε, Eccentricityε, Angleε float64 GenericCL } @@ -263,9 +263,9 @@ func NewOptimalΔOrbit(target Orbit, method ControlLawType, laws []ControlLaw) * cl.cleared = false cl.method = method cl.oTgta, cl.oTgte, cl.oTgti, cl.oTgtΩ, cl.oTgtω, cl.oTgtν, _, _, _ = target.Elements() - cl.distanceε = distanceε - cl.eccentricityε = eccentricityε - cl.angleε = angleε + cl.Distanceε = distanceε + cl.Eccentricityε = eccentricityε + cl.Angleε = angleε if len(laws) == 0 { laws = []ControlLaw{OptiΔaCL, OptiΔeCL, OptiΔiCL, OptiΔΩCL, OptiΔωCL} } @@ -288,9 +288,9 @@ func (cl *OptimalΔOrbit) SetTarget(target Orbit) { // SetEpsilons changes the target of this optimal control func (cl *OptimalΔOrbit) SetEpsilons(distanceε, eccentricityε, angleε float64) { - cl.distanceε = distanceε - cl.eccentricityε = eccentricityε - cl.angleε = angleε + cl.Distanceε = distanceε + cl.Eccentricityε = eccentricityε + cl.Angleε = angleε } func (cl *OptimalΔOrbit) String() string { @@ -306,19 +306,19 @@ func (cl *OptimalΔOrbit) Control(o Orbit) []float64 { if len(cl.controls) == 5 { // Let's populate this with the appropriate laws, so we're resetting it. cl.controls = make([]ThrustControl, 0) - if !floats.EqualWithinAbs(cl.oInita, cl.oTgta, distanceε) { + if !floats.EqualWithinAbs(cl.oInita, cl.oTgta, cl.Distanceε) { cl.controls = append(cl.controls, NewOptimalThrust(OptiΔaCL, "Δa")) } - if !floats.EqualWithinAbs(cl.oInite, cl.oTgte, eccentricityε) { + if !floats.EqualWithinAbs(cl.oInite, cl.oTgte, cl.Eccentricityε) { cl.controls = append(cl.controls, NewOptimalThrust(OptiΔeCL, "Δe")) } - if !floats.EqualWithinAbs(cl.oIniti, cl.oTgti, angleε) { + if !floats.EqualWithinAbs(cl.oIniti, cl.oTgti, cl.Angleε) { cl.controls = append(cl.controls, NewOptimalThrust(OptiΔiCL, "Δi")) } - if !floats.EqualWithinAbs(cl.oInitΩ, cl.oTgtΩ, angleε) { + if !floats.EqualWithinAbs(cl.oInitΩ, cl.oTgtΩ, cl.Angleε) { cl.controls = append(cl.controls, NewOptimalThrust(OptiΔΩCL, "ΔΩ")) } - if !floats.EqualWithinAbs(cl.oInitω, cl.oTgtω, angleε) { + if !floats.EqualWithinAbs(cl.oInitω, cl.oTgtω, cl.Angleε) { cl.controls = append(cl.controls, NewOptimalThrust(OptiΔωCL, "Δω")) } } @@ -330,8 +330,11 @@ func (cl *OptimalΔOrbit) Control(o Orbit) []float64 { switch cl.method { case Ruggiero: factor := func(oscul, init, target, tol float64) float64 { - if floats.EqualWithinAbs(init, target, tol) || floats.EqualWithinAbs(oscul, target, tol) { - return 0 // Don't want no NaNs now. + if floats.EqualWithinAbs(oscul, target, tol) { + return 0 + } + if floats.EqualWithinAbs(init, target, tol) { + init += tol // Adding a small error to avoid NaN while still making the correction } return (target - oscul) / math.Abs(target-init) } @@ -343,27 +346,27 @@ func (cl *OptimalΔOrbit) Control(o Orbit) []float64 { oscul = a init = cl.oInita target = cl.oTgta - tol = distanceε + tol = cl.Distanceε case OptiΔeCL: oscul = e init = cl.oInite target = cl.oTgte - tol = eccentricityε + tol = cl.Eccentricityε case OptiΔiCL: oscul = i init = cl.oIniti target = cl.oTgti - tol = angleε + tol = cl.Angleε case OptiΔΩCL: oscul = Ω init = cl.oInitΩ target = cl.oTgtΩ - tol = angleε + tol = cl.Angleε case OptiΔωCL: oscul = ω init = cl.oInitω target = cl.oTgtω - tol = angleε + tol = cl.Angleε } // XXX: This summation may be wrong: |\sum x_i| != \sum |x_i|. if fact := factor(oscul, init, target, tol); fact != 0 { @@ -378,7 +381,7 @@ func (cl *OptimalΔOrbit) Control(o Orbit) []float64 { // Note that, as described in Hatten MSc. thesis, the summing method only // works one way (because of the δO^2) per OE. So I added the sign function // to fix it. - dε, eε, aε := o.epsilons() + //dε, eε, aε := o.epsilons() for _, ctrl := range cl.controls { var weight, δO float64 p := o.SemiParameter() @@ -387,19 +390,19 @@ func (cl *OptimalΔOrbit) Control(o Orbit) []float64 { switch ctrl.Type() { case OptiΔaCL: δO = cl.oTgta - a - if math.Abs(δO) < dε { + if math.Abs(δO) < cl.Distanceε { δO = 0 } weight = Sign(δO) * math.Pow(h, 2) / (4 * math.Pow(a, 4) * math.Pow(1+e, 2)) case OptiΔeCL: δO = cl.oTgte - e - if math.Abs(δO) < eε { + if math.Abs(δO) < cl.Eccentricityε { δO = 0 } weight = Sign(δO) * math.Pow(h, 2) / (4 * math.Pow(p, 2)) case OptiΔiCL: δO = cl.oTgti - i - if math.Abs(δO) < aε { + if math.Abs(δO) < cl.Angleε { δO = 0 } weight = Sign(δO) * math.Pow((h+e*h*math.Cos(ω+math.Asin(e*sinω)))/(p*(math.Pow(e*sinω, 2)-1)), 2) @@ -409,7 +412,7 @@ func (cl *OptimalΔOrbit) Control(o Orbit) []float64 { // Enforce short path to correct angle. δO *= -1 } - if math.Abs(δO) < aε { + if math.Abs(δO) < cl.Angleε { δO = 0 } weight = Sign(δO) * math.Pow((h*math.Sin(i)*(e*math.Sin(ω+math.Asin(e*cosω))-1))/(p*(1-math.Pow(e*cosω, 2))), 2) @@ -419,7 +422,7 @@ func (cl *OptimalΔOrbit) Control(o Orbit) []float64 { // Enforce short path to correct angle. δO *= -1 } - if math.Abs(δO) < aε { + if math.Abs(δO) < cl.Angleε { δO = 0 } weight = Sign(δO) * (math.Pow(e*h, 2) / (4 * math.Pow(p, 2))) * (1 - math.Pow(e, 2)/4) diff --git a/waypoints.go b/waypoints.go index 2066bc6..b899491 100644 --- a/waypoints.go +++ b/waypoints.go @@ -171,7 +171,7 @@ func (wp *OrbitTarget) SetEpsilons(distanceε, eccentricityε, angleε float64) // ThrustDirection implements the optimal orbit target. func (wp *OrbitTarget) ThrustDirection(o Orbit, dt time.Time) (ThrustControl, bool) { - if ok, err := wp.target.Equals(o); ok { + if ok, err := wp.target.EqualsWithin(o, wp.ctrl.Distanceε, wp.ctrl.Eccentricityε, wp.ctrl.Angleε); ok { wp.cleared = true } else if wp.ctrl.cleared { fmt.Printf("[WARNING] OrbitTarget reached @%s *but* %s: %s\n", dt, err, o.String())