Features/gauss method (#229)

sylvain-guillet · web-flow · commit 2d81dedb5638 · 2024-12-12T21:37:14.000+01:00
* Add Gauss method

* Add tests

* Add Raphson newton

* Add bissection

* Fix gauss

* Use expectedCenter of motion for Rn values

* Adapt tests and revert gauss method to seconds

* Fix gauss implementation

* Logs

* Add tests and validate method

* Reduce coeff for gauss's method

* Use GM AU3/D2

* Find COBE

* Geo and cobe work

* add logs

* fix tests

* Add threshold for coplanar and global arc

* Evaluate reliability

* Improve reliability

* Clean code

* Upgrade version
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics.CLI/IO.Astrodynamics.CLI.csproj b/IO.Astrodynamics.Net/IO.Astrodynamics.CLI/IO.Astrodynamics.CLI.csproj
@@ -9,7 +9,7 @@
         <FileVersion>0.0.1</FileVersion>
         <PackAsTool>true</PackAsTool>
         <ToolCommandName>astro</ToolCommandName>
-        <Version>0.6.2.1</Version>
+        <Version>0.6.3.1</Version>
         <Title>Astrodynamics command line interface</Title>
         <Authors>Sylvain Guillet</Authors>
         <Description>This CLI allows end user to exploit IO.Astrodynamics framework </Description>
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics.Tests/OrbitalParameters/FindOrbitalParametersFromObservations.cs b/IO.Astrodynamics.Net/IO.Astrodynamics.Tests/OrbitalParameters/FindOrbitalParametersFromObservations.cs
@@ -0,0 +1,101 @@
+using System;
+using IO.Astrodynamics.Body;
+using IO.Astrodynamics.Coordinates;
+using IO.Astrodynamics.Math;
+using IO.Astrodynamics.OrbitalParameters;
+using IO.Astrodynamics.SolarSystemObjects;
+using IO.Astrodynamics.Surface;
+using Xunit;
+
+namespace IO.Astrodynamics.Tests.OrbitalParameters;
+
+public class InitialOrbitDeterminationTests
+{
+    public InitialOrbitDeterminationTests()
+    {
+        API.Instance.LoadKernels(Constants.SolarSystemKernelPath);
+    }
+
+    [Fact]
+    public void GeosynchronousObject()
+    {
+        for (int i = 1; i <= 5; i++)
+        {
+            var timespan = TimeSpan.FromMinutes(i * 2);
+            Site site = new Site(80, "MyStation", TestHelpers.EarthAtJ2000, new Planetodetic(0.0, 45.0 * Constants.DEG_RAD, 0.0));
+
+            var e2 = new TimeSystem.Time(2024, 1, 2);
+
+            var e1 = e2 - timespan;
+            var e3 = e2 + timespan;
+            var referenceOrbit = new KeplerianElements(
+                semiMajorAxis: 42164000.0, // m
+                eccentricity: 0.0,
+                inclination: 15.0 * Constants.DEG_RAD,
+                rigthAscendingNode: 45.0 * Constants.DEG_RAD,
+                argumentOfPeriapsis: 0.0,
+                meanAnomaly: 45.0 * Constants.DEG_RAD, // Décalage de 45 degrés
+                observer: TestHelpers.EarthAtJ2000,
+                frame: Frames.Frame.ICRF,
+                epoch: e2
+            );
+            var obs1 = referenceOrbit.AtEpoch(e1).RelativeTo(site, Aberration.LT);
+            var obs2 = referenceOrbit.AtEpoch(e2).RelativeTo(site, Aberration.LT);
+            var obs3 = referenceOrbit.AtEpoch(e3).RelativeTo(site, Aberration.LT);
+            var orbitalParams =
+                InitialOrbitDetermination.CreateFromObservation_Gauss(obs1.ToEquatorial(), obs2.ToEquatorial(), obs3.ToEquatorial(), site,
+                    PlanetsAndMoons.EARTH_BODY, 42000000.0);
+            var deltaRange = 100.0 * (orbitalParams.OrbitalParameters.ToStateVector().Position - referenceOrbit.ToStateVector().Position).Magnitude() /
+                             referenceOrbit.ToStateVector().Position.Magnitude();
+            var deltaVelocity = 100.0 * (orbitalParams.OrbitalParameters.ToStateVector().Velocity - referenceOrbit.ToStateVector().Velocity).Magnitude() /
+                                referenceOrbit.ToStateVector().Velocity.Magnitude();
+            Assert.True(deltaRange < 0.02);
+            Assert.True(deltaVelocity < 0.02);
+        }
+    }
+
+    [Fact]
+    public void PlanetObject()
+    {
+            var timespan = TimeSpan.FromDays(10);
+            Site site = new Site(13, "DSS-13", TestHelpers.EarthAtJ2000);
+
+            var e2 = new TimeSystem.Time(2023, 3, 1);
+            var e1 = e2 - timespan;
+            var e3 = e2 + timespan;
+            var target = new Barycenter(4);
+            var obs1 = target.GetEphemeris(e1, site, Frames.Frame.ICRF, Aberration.LT);
+            var obs2 = target.GetEphemeris(e2, site, Frames.Frame.ICRF, Aberration.LT);
+            var obs3 = target.GetEphemeris(e3, site, Frames.Frame.ICRF, Aberration.LT);
+            var referenceOrbit = target.GetEphemeris(e2, TestHelpers.Sun, Frames.Frame.ICRF, Aberration.LT);
+            var orbitalParams =
+                InitialOrbitDetermination.CreateFromObservation_Gauss(obs1.ToEquatorial(), obs2.ToEquatorial(), obs3.ToEquatorial(), site,
+                    Stars.SUN_BODY, 350000000000.58063);
+            
+            var deltaRange = 100.0 * (orbitalParams.OrbitalParameters.ToStateVector().Position - referenceOrbit.ToStateVector().Position).Magnitude() /
+                             referenceOrbit.ToStateVector().Position.Magnitude();
+            var deltaVelocity = 100.0 * (orbitalParams.OrbitalParameters.ToStateVector().Velocity - referenceOrbit.ToStateVector().Velocity).Magnitude() /
+                                referenceOrbit.ToStateVector().Velocity.Magnitude();
+            
+            Assert.True(deltaRange < 0.06);
+            Assert.True(deltaVelocity < 0.15);
+    }
+
+    [Fact]
+    public void COBE()
+    {
+        var ut1 = new TimeSystem.Time(2000, 11, 6, 22, 31, 29, 0, 0, TimeSystem.TimeFrame.UTCFrame);
+        var ut2 = new TimeSystem.Time(2000, 11, 6, 22, 34, 30, 0, 0, TimeSystem.TimeFrame.UTCFrame);
+        var ut3 = new TimeSystem.Time(2000, 11, 6, 22, 37, 30, 0, 0, TimeSystem.TimeFrame.UTCFrame);
+        var site = new Site(99, "Maryland university", TestHelpers.EarthAtJ2000, new Planetodetic(-76.95667 * Constants.DEG_RAD, 39.00167 * Constants.DEG_RAD, 53.0));
+        var obs1 = new Equatorial(-16.3 * Astrodynamics.Constants.Deg2Rad, 327.0 * Astrodynamics.Constants.Deg2Rad, 7250000.0, ut1);
+        var obs2 = new Equatorial(46.9 * Astrodynamics.Constants.Deg2Rad, 318.5 * Astrodynamics.Constants.Deg2Rad, 7250000.0, ut2);
+        var obs3 = new Equatorial(76.1 * Astrodynamics.Constants.Deg2Rad, 165.75 * Astrodynamics.Constants.Deg2Rad, 7250000.0, ut3);
+        var orbitalParams =
+            InitialOrbitDetermination.CreateFromObservation_Gauss(obs1, obs2, obs3, site, PlanetsAndMoons.EARTH_BODY, 7_250_000.0);
+        StateVector referenceOrbit = new(new Vector3(3520477.0118993367, -4310404.2221997585, 4645118.5764311915),
+            new Vector3(-4026.2486947722973, 2615.67401249017, 5468.113004203227),
+            TestHelpers.EarthAtJ2000, ut2, Frames.Frame.ICRF);
+        Assert.Equal(referenceOrbit, orbitalParams.OrbitalParameters.ToStateVector(), TestHelpers.StateVectorComparer);
+    }
+}
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/Coordinates/Equatorial.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/Coordinates/Equatorial.cs
@@ -47,5 +47,13 @@ public Vector3 ToCartesian()
             double z = Distance * System.Math.Sin(Declination);
             return new Vector3(x, y, z);
         }
+
+        public Vector3 ToDirection()
+        {
+            double x = System.Math.Cos(Declination) * System.Math.Cos(RightAscension);
+            double y = System.Math.Cos(Declination) * System.Math.Sin(RightAscension);
+            double z = System.Math.Sin(Declination);
+            return new Vector3(x, y, z);
+        }
     }
 }
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/IO.Astrodynamics.nuspec b/IO.Astrodynamics.Net/IO.Astrodynamics/IO.Astrodynamics.nuspec
@@ -4,7 +4,7 @@
         <id>IO.Astrodynamics</id>
         <authors>Sylvain Guillet</authors>
         <copyright>Sylvain Guillet</copyright>
-        <version>6.2.0</version>
+        <version>6.3.0</version>
         <title>Astrodynamics framework</title>
         <icon>images\dragonfly-dark-trans.png</icon>
         <readme>docs\README.md</readme>
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/Math/Bissection.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/Math/Bissection.cs
@@ -0,0 +1,47 @@
+using System;
+
+public class BisectionMethod
+{
+    public static double Solve(
+        Func<double, double> f,     // Your polynomial function
+        double a,                   // Left endpoint
+        double b,                   // Right endpoint
+        double tolerance = 1E-12,   // Matching your tolerance
+        int maxIterations = 1000)   // Matching your max iterations
+    {
+        if (f(a) * f(b) >= 0)
+        {
+            throw new ArgumentException("Function must have different signs at endpoints a and b");
+        }
+
+        double c = a;
+        int iterations = 0;
+
+        while ((b - a) > tolerance && iterations < maxIterations)
+        {
+            c = (a + b) / 2;
+            double fc = f(c);
+
+            if (Math.Abs(fc) < tolerance)
+                break;
+
+            if (f(a) * fc < 0)
+            {
+                b = c;
+            }
+            else
+            {
+                a = c;
+            }
+
+            iterations++;
+        }
+
+        if (iterations >= maxIterations)
+        {
+            throw new ArgumentException($"Failed to converge after {maxIterations} iterations");
+        }
+
+        return c;
+    }
+}
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/Math/NewtonRaphson.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/Math/NewtonRaphson.cs
@@ -0,0 +1,85 @@
+using System;
+
+namespace IO.Astrodynamics.Math;
+
+/// <summary>
+/// Newton-Raphson method for finding the root of a function. 
+/// </summary>
+public class NewtonRaphson
+{
+    /// <summary>
+    /// Solve the equation f(x) = 0 using the Newton-Raphson method. 
+    /// </summary>
+    /// <param name="function"></param>
+    /// <param name="derivative"></param>
+    /// <param name="initialGuess"></param>
+    /// <param name="tolerance"></param>
+    /// <param name="maxIterations"></param>
+    /// <returns></returns>
+    /// <exception cref="Exception"></exception>
+    public static double Solve(
+        Func<double, double> function,
+        Func<double, double> derivative,
+        double initialGuess,
+        double tolerance = 1e-6,
+        int maxIterations = 100)
+    {
+        double x = initialGuess;
+
+        for (int i = 0; i < maxIterations; i++)
+        {
+            double fValue = function(x);
+            double fDerivative = derivative(x);
+
+            if (System.Math.Abs(fDerivative) < 1e-12) // Avoid division by zero or near-zero derivatives
+                throw new Exception($"Derivative too small (f'(x) = {fDerivative}) at iteration {i}. Adjust initial guess or problem scaling.");
+
+            // Update the root estimate using Newton-Raphson formula
+            double xNew = x - fValue / fDerivative;
+
+            // Check for convergence (absolute and relative tolerance)
+            if (System.Math.Abs(xNew - x) < tolerance || System.Math.Abs(xNew - x) < tolerance * System.Math.Abs(xNew))
+                return xNew;
+
+            if (double.IsNaN(xNew) || double.IsInfinity(xNew))
+                throw new Exception("Newton-Raphson method diverged.");
+
+            x = xNew;
+        }
+
+
+        throw new Exception("Newton-Raphson method did not converge within the maximum number of iterations.");
+    }
+    
+    public static double BoundedNewtonRaphson(Func<double, double> f, Func<double, double> df, 
+        double x0, double min, double max, double tolerance, int maxIterations)
+    {
+        double x = x0;
+        for (int i = 0; i < maxIterations; i++)
+        {
+            double fx = f(x);
+            if (System.Math.Abs(fx) < tolerance)
+                return x;
+
+            double dfx = df(x);
+            if (System.Math.Abs(dfx) < 1e-10)
+                break;
+
+            double step = fx / dfx;
+            double newX = x - step;
+        
+            // Bound the solution
+            if (newX < min)
+                newX = min;
+            else if (newX > max)
+                newX = max;
+
+            if (System.Math.Abs(newX - x) < tolerance)
+                return newX;
+
+            x = newX;
+        }
+    
+        throw new Exception("Failed to converge within maximum iterations");
+    }
+}
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/Math/Solver.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/Math/Solver.cs
@@ -0,0 +1,27 @@
+using System;
+
+namespace IO.Astrodynamics.Math;
+
+public class Solver
+{
+    /// <summary>
+    /// Solve the quadratic equation ax^2 + bx + c = 0.
+    /// </summary>
+    /// <param name="a"></param>
+    /// <param name="b"></param>
+    /// <param name="c"></param>
+    /// <returns></returns>
+    /// <exception cref="InvalidOperationException"></exception>
+    public static double SolveQuadratic(double a, double b, double c)
+    {
+        double discriminant = b * b - 4 * a * c;
+        if (discriminant < 0)
+            throw new InvalidOperationException("No real roots for the polynomial.");
+
+        double root1 = (-b + System.Math.Sqrt(discriminant)) / (2 * a);
+        double root2 = (-b - System.Math.Sqrt(discriminant)) / (2 * a);
+
+        // Select the positive root (physically meaningful solution)
+        return System.Math.Max(root1, root2);
+    }
+}
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/Math/Vector3.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/Math/Vector3.cs
@@ -20,7 +20,12 @@ public Vector3(double x, double y, double z)
 
         public double Magnitude()
         {
-            return System.Math.Sqrt(X * X + Y * Y + Z * Z);
+            return System.Math.Sqrt(MagnitudeSquared());
+        }
+        
+        public double MagnitudeSquared()
+        {
+            return X * X + Y * Y + Z * Z;
         }
 
         public Vector3 Normalize()
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/InitialOrbitDetermination.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/InitialOrbitDetermination.cs
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/KeplerianElements.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/KeplerianElements.cs
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/OrbitDetermination.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/OrbitDetermination.cs
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/OrbitalParameters.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/OrbitalParameters.cs

Original file line number	Diff line number	Diff line change
`@@ -47,5 +47,13 @@ public Vector3 ToCartesian()`
`47`	`47`	`double z = Distance * System.Math.Sin(Declination);`
`48`	`48`	`return new Vector3(x, y, z);`
`49`	`49`	`}`
	`50`	`+`
	`51`	`+ public Vector3 ToDirection()`
	`52`	`+ {`
	`53`	`+ double x = System.Math.Cos(Declination) * System.Math.Cos(RightAscension);`
	`54`	`+ double y = System.Math.Cos(Declination) * System.Math.Sin(RightAscension);`
	`55`	`+ double z = System.Math.Sin(Declination);`
	`56`	`+ return new Vector3(x, y, z);`
	`57`	`+ }`
`50`	`58`	`}`
`51`	`59`	`}`
Original file line number	Diff line number	Diff line change
`@@ -20,7 +20,12 @@ public Vector3(double x, double y, double z)`
`20`	`20`
`21`	`21`	`public double Magnitude()`
`22`	`22`	`{`
`23`		`- return System.Math.Sqrt(X * X + Y * Y + Z * Z);`
	`23`	`+ return System.Math.Sqrt(MagnitudeSquared());`
	`24`	`+ }`
	`25`	`+`
	`26`	`+ public double MagnitudeSquared()`
	`27`	`+ {`
	`28`	`+ return X * X + Y * Y + Z * Z;`
`24`	`29`	`}`
`25`	`30`
`26`	`31`	`public Vector3 Normalize()`