/// <summary> /// 用反替换法求解向量x[]的LSQ问题 /// 通过反向替换解决向量x []的LSQ问题 /// Solve the LSQ problem for vector x[] by backsubstitution /// </summary> /// <param name="x"></param> public void Solve(Geo.Algorithm.Vector x) { // Variables int i, j; i = j = 0; double Sum = 0.0; // Check for singular matrix for (i = 0; i < ParamCount; i++) { if (SquareRoot[i, i] == 0.0) { Console.Write(" ERROR: Singular matrix R in LSQ::Solve()");// } } ; // Solve Rx=d for x_n,...,x_1 by backsubstitution x[ParamCount - 1] = RightHandSide[ParamCount - 1] / SquareRoot[ParamCount - 1, ParamCount - 1]; for (i = ParamCount - 2; i >= 0; i--) { Sum = 0.0; for (j = i + 1; j < ParamCount; j++) { Sum += SquareRoot[i, j] * x[j]; } x[i] = (RightHandSide[i] - Sum) / SquareRoot[i, i]; } ; }
/// <summary> /// 计算大气延迟。 Computes the acceleration due to the atmospheric drag. /// </summary> /// <param name="Mjd_TT">Mjd_TT Terrestrial Time (Modified Julian Date)</param> /// <param name="satXyz">r Satellite position vector in the inertial system [m]</param> /// <param name="satVelocity"> v Satellite velocity vector in the inertial system [m/s]</param> /// <param name="transMatrix"> T Transformation matrix to true-of-date inertial system</param> /// <param name="Area">Area Cross-section [m^2]</param> /// <param name="mass"> mass Spacecraft mass [kg]</param> /// <param name="atmosDragCoeff"> coefOfDrag Drag coefficient</param> /// <returns>Acceleration (a=d^2r/dt^2) [m/s^2]</returns> public static Geo.Algorithm.Vector AccelerDragOfAtmos(double Mjd_TT, Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector satVelocity, Matrix transMatrix, double Area, double mass, double atmosDragCoeff) { // Earth angular velocity vector [rad/s] double[] Data_omega = { 0.0, 0.0, 7.29212e-5 }; Geo.Algorithm.Vector omega = new Geo.Algorithm.Vector(Data_omega, 3); // Variables double v_abs, dens; Geo.Algorithm.Vector r_tod = new Geo.Algorithm.Vector(3), v_tod = new Geo.Algorithm.Vector(3); Geo.Algorithm.Vector v_rel = new Geo.Algorithm.Vector(3), a_tod = new Geo.Algorithm.Vector(3); Matrix T_trp = new Matrix(3, 3); // Transformation matrix to ICRF/EME2000 system T_trp = transMatrix.Transpose(); // Position and velocity in true-of-date system r_tod = transMatrix * satXyz; v_tod = transMatrix * satVelocity; // Velocity relative to the Earth's atmosphere v_rel = v_tod - omega.Cross3D(r_tod); v_abs = v_rel.Norm(); // Atmospheric density due to modified Harris-Priester model dens = TerrestrialUtil.AtmosDensity_HP(Mjd_TT, r_tod); // Acceleration a_tod = -0.5 * atmosDragCoeff * (Area / mass) * dens * v_abs * v_rel; return(T_trp * a_tod); }
/// <summary> ///计算航天器是否在太阳光照中。1 为是, 0 为否。 可用于计算太阳光压。 /// Computes the fractional illumination of a spacecraft in the /// vicinity of the Earth assuming a cylindrical shadow model /// </summary> /// <param name="satXyz_m">Spacecraft position vector [m]</param> /// <param name="sunXyz_m"> Sun position vector [m]</param> /// <returns> Illumination factor: /// nu=0 Spacecraft in Earth shadow /// nu=1 Spacecraft fully illuminated by the Sun</returns> public static double Illumination(Geo.Algorithm.Vector satXyz_m, Geo.Algorithm.Vector sunXyz_m) { Geo.Algorithm.Vector e_Sun = sunXyz_m.DirectionUnit(); // Sun direction unit vector double s = satXyz_m.Dot(e_Sun); // Projection of s/c position return((s > 0 || (satXyz_m - s * e_Sun).Norm() > OrbitConsts.RadiusOfEarth) ? 1.0 : 0.0); }
/// <summary> /// 本地空间直角坐标到极坐标的转换. Computes azimuth and elevation from local tangent coordinates /// </summary> /// <param name="localEnz"> s Topocentric local tangent coordinates (East-North-Zenith frame)</param> /// <param name="azimuthRad"> A Azimuth [rad]</param> /// <param name="elevationRad"> E Elevation [rad]</param> /// <param name="Range"> Range [m]</param> public static void LocalEnzToPolar(Geo.Algorithm.Vector localEnz, out double azimuthRad, out double elevationRad, out double range) { range = localEnz.Norm(); azimuthRad = Math.Atan2(localEnz[0], localEnz[1]); azimuthRad = ((azimuthRad < 0.0) ? azimuthRad + OrbitConsts.TwoPI : azimuthRad); elevationRad = Math.Atan(localEnz[2] / Math.Sqrt(localEnz[0] * localEnz[0] + localEnz[1] * localEnz[1])); }
/// <summary> /// 计算绕地航天器主要的加速度。 /// Computes the acceleration of an Earth orbiting satellite due to /// - the Earth's harmonic gravity field, /// - the gravitational perturbations of the Sun and Moon /// - the solar radiation pressure and /// - the atmospheric drag /// </summary> /// <param name="Mjd_TT"> Mjd_TT Terrestrial Time (Modified Julian Date)</param> /// <param name="satXyzIcrf"> r 卫星在国际天球参考框架的位置 Satellite position vector in the ICRF/EME2000 system</param> /// <param name="satVelocityIcrf"> v 卫星在国际天球参考框架的速度 Satellite velocity vector in the ICRF/EME2000 system</param> /// <param name="Area"> Area Cross-section </param> /// <param name="mass"> mass 质量 Spacecraft mass</param> /// <param name="solarRadiPresCoeff"> coefOfRadiation 光压系数 Radiation pressure coefficient</param> /// <param name="atmDragCoefficient"> coefOfDrag 大气阻力系数 Drag coefficient</param> /// <returns> Acceleration (a=d^2r/dt^2) in the ICRF/EME2000 system</returns> public static Geo.Algorithm.Vector AccelerOfMainForces(double Mjd_TT, Geo.Algorithm.Vector satXyzIcrf, Geo.Algorithm.Vector satVelocityIcrf, double Area, double mass, double solarRadiPresCoeff, double atmDragCoefficient) { double Mjd_UT1; Geo.Algorithm.Vector a = new Geo.Algorithm.Vector(3), r_Sun = new Geo.Algorithm.Vector(3), r_Moon = new Geo.Algorithm.Vector(3); Matrix T = new Matrix(3, 3), E = new Matrix(3, 3); // Acceleration due to harmonic gravity field Mjd_UT1 = Mjd_TT; T = IERS.NutMatrix(Mjd_TT) * IERS.PrecessionMatrix(MJD_J2000, Mjd_TT); E = IERS.GreenwichHourAngleMatrix(Mjd_UT1) * T; a = AccelerOfHarmonicGraviFiled(satXyzIcrf, E, Grav.GM, Grav.R_ref, Grav.CS, Grav.n_max, Grav.m_max); // Luni-solar perturbations r_Sun = CelestialUtil.Sun(Mjd_TT); r_Moon = CelestialUtil.Moon(Mjd_TT); a += AccelerOfPointMass(satXyzIcrf, r_Sun, OrbitConsts.GM_Sun); a += AccelerOfPointMass(satXyzIcrf, r_Moon, OrbitConsts.GM_Moon); // Solar radiation pressure a += CelestialUtil.Illumination(satXyzIcrf, r_Sun) * AccelerOfSolarRadiPressure(satXyzIcrf, r_Sun, Area, mass, solarRadiPresCoeff, OrbitConsts.PressureOfSolarRadiationPerAU, AU); // Atmospheric drag a += AccelerDragOfAtmos(Mjd_TT, satXyzIcrf, satVelocityIcrf, T, Area, mass, atmDragCoefficient); // Acceleration return(a); }
/// <summary> /// Ephemeris computation /// </summary> /// <param name="Y0"></param> /// <param name="N_Step"></param> /// <param name="Step"></param> /// <param name="p"></param> /// <param name="Eph"></param> static void Ephemeris(Geo.Algorithm.Vector Y0, int N_Step, double Step, ForceModelOption p, Geo.Algorithm.Vector[] Eph) { int i; double t, t_end; double relerr, abserr; // Accuracy requirements DeIntegrator Orb = new DeIntegrator(Deriv, 6, p); // Object for integrating the eq. of motion Geo.Algorithm.Vector Y = new Geo.Algorithm.Vector(Y0); relerr = 1.0e-13; abserr = 1.0e-6; t = 0.0; // Y = Y0; Orb.Init(t, relerr, abserr); for (i = 0; i <= N_Step; i++) { t_end = Step * i; var Yresult = Orb.Integ(t_end, Y); Eph[i] = Yresult; } ; //var prevObj = Eph[0]; //Console.WriteLine(prevObj); //var last = Eph[Eph.Length -1]; //Console.WriteLine(last); }
//------------------------------------------------------------------------------ // // Elements // // Purpose: // // Computes the osculating Keplerian elements from the satellite state vector // for elliptic orbits // // Input/Output: // // GM Gravitational coefficient // (gravitational constant * mass of central body) // y State vector (x,y,z,vx,vy,vz) // <return> Keplerian elements (a,e,i,Omega,omega,M) with // a Semimajor axis // e Eccentricity // i Inclination [rad] // Omega Longitude of the ascending node [rad] // omega Argument of pericenter [rad] // M Mean anomaly [rad] // // Notes: // // The state vector and GM must be given in consistent units, // e.g. [m], [m/s] and [m^3/s^2]. The resulting unit of the semimajor // axis is implied by the unity of y, e.g. [m]. // // The function cannot be used with state vectors describing a circular // or non-inclined orbit. // //------------------------------------------------------------------------------ /// <summary> /// 由某点的位置和速度,求解开普勒轨道根数。 /// </summary> /// <param name="GM"> Gravitational coefficien<t/param> /// <param name="y"> State vector (x,y,z,vx,vy,vz) </param> /// <returns></returns> public static Geo.Algorithm.Vector Elements(double GM, Geo.Algorithm.Vector stateVector) { // Variables Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(3), v = new Geo.Algorithm.Vector(3), h = new Geo.Algorithm.Vector(3); double H, u, R; double eCosE, eSinE, e2, E, nu; double a, e, i, Omega, omega, M; r = stateVector.Slice(0, 2); // Position v = stateVector.Slice(3, 5); // Velocity h = r.Cross3D(v); // Areal velocity H = h.Norm(); Omega = Math.Atan2(h[0], -h[1]); // Long. ascend. node Omega = MathUtil.Modulo(Omega, OrbitConsts.TwoPI); i = Math.Atan2(Math.Sqrt(h[0] * h[0] + h[1] * h[1]), h[2]); // Inclination u = Math.Atan2(r[2] * H, -r[0] * h[1] + r[1] * h[0]); // Arg. of latitude R = r.Norm(); // Distance a = 1.0 / (2.0 / R - v.Dot(v) / GM); // Semi-major axis eCosE = 1.0 - R / a; // e*cos(E) eSinE = r.Dot(v) / Math.Sqrt(GM * a); // e*Math.Sin(E) e2 = eCosE * eCosE + eSinE * eSinE; e = Math.Sqrt(e2); // Eccentricity E = Math.Atan2(eSinE, eCosE); // Eccentric anomaly M = MathUtil.Modulo(E - eSinE, OrbitConsts.TwoPI); // Mean anomaly nu = Math.Atan2(Math.Sqrt(1.0 - e2) * eSinE, eCosE - e2); // True anomaly omega = MathUtil.Modulo(u - nu, OrbitConsts.TwoPI); // Arg. of perihelion // Keplerian elements vector return(new Geo.Algorithm.Vector(a, e, i, Omega, omega, M)); }
//------------------------------------------------------------------------------ // // TwoBody // // Purpose: // // Propagates a given state vector and computes the state transition matrix // for elliptical Keplerian orbits // // Input/Output: // // GM Gravitational coefficient // (gravitational constant * mass of central body) // Y0 Epoch state vector (x,y,z,vx,vy,vz)_0 // dt Time since epoch // Y State vector (x,y,z,vx,vy,vz) // dYdY0 State transition matrix d(x,y,z,vx,vy,vz)/d(x,y,z,vx,vy,vz)_0 // // Notes: // // The state vector, dt and GM must be given in consistent units, // e.g. [m], [m/s] and [m^3/s^2]. The resulting units of length and velocity // are implied by the units of GM, e.g. [m] and [m/s]. // // Due to the internal use of Keplerian elements, the function cannot be // used with epoch state vectors describing a circular or non-inclined orbit. // //------------------------------------------------------------------------------ /// <summary> /// 二体问题计算Propagates a given state vector and computes the state transition matrix /// for elliptical Keplerian orbits /// </summary> /// <param name="GM">Gravitational coefficient (gravitational constant * mass of central body) </param> /// <param name="Y0">Epoch state vector (x,y,z,vx,vy,vz)_0</param> /// <param name="dt"> Time since epoch</param> /// <param name="Y">State vector (x,y,z,vx,vy,vz)</param> /// <param name="dYdY0"> State transition matrix d(x,y,z,vx,vy,vz)/d(x,y,z,vx,vy,vz)_0</param> public static void TwoBody(double GM, Geo.Algorithm.Vector Y0, double dt, ref Geo.Algorithm.Vector Y, ref Matrix dYdY0) { // Variables int k; double a, e, i, n, sqe2, naa; double P_aM, P_eM, P_eo, P_io, P_iO; Geo.Algorithm.Vector A0 = new Geo.Algorithm.Vector(6); Matrix dY0dA0 = new Matrix(6, 6), dYdA0 = new Matrix(6, 6), dA0dY0 = new Matrix(6, 6); // Orbital elements at epoch A0 = Elements(GM, Y0); a = A0[0]; e = A0[1]; i = A0[2]; n = Math.Sqrt(GM / (a * a * a)); // Propagated state Y = State(GM, A0, dt); // State vector partials w.r.t epoch elements dY0dA0 = StatePartials(GM, A0, 0.0); dYdA0 = StatePartials(GM, A0, dt); // Poisson brackets sqe2 = Math.Sqrt((1.0 - e) * (1.0 + e)); naa = n * a * a; P_aM = -2.0 / (n * a); // P(a,M) = -P(M,a) P_eM = -(1.0 - e) * (1.0 + e) / (naa * e); // P(e,M) = -P(M,e) P_eo = +sqe2 / (naa * e); // P(e,omega) = -P(omega,e) P_io = -1.0 / (naa * sqe2 * Math.Tan(i)); // P(i,omega) = -P(omega,i) P_iO = +1.0 / (naa * sqe2 * Math.Sin(i)); // P(i,Omega) = -P(Omega,i) // Partials of epoch elements w.r.t. epoch state for (k = 0; k < 3; k++) { dA0dY0[0, k] = +P_aM * dY0dA0[k + 3, 5]; dA0dY0[0, k + 3] = -P_aM * dY0dA0[k, 5]; dA0dY0[1, k] = +P_eo * dY0dA0[k + 3, 4] + P_eM * dY0dA0[k + 3, 5]; dA0dY0[1, k + 3] = -P_eo * dY0dA0[k, 4] - P_eM * dY0dA0[k, 5]; dA0dY0[2, k] = +P_iO * dY0dA0[k + 3, 3] + P_io * dY0dA0[k + 3, 4]; dA0dY0[2, k + 3] = -P_iO * dY0dA0[k, 3] - P_io * dY0dA0[k, 4]; dA0dY0[3, k] = -P_iO * dY0dA0[k + 3, 2]; dA0dY0[3, k + 3] = +P_iO * dY0dA0[k, 2]; dA0dY0[4, k] = -P_eo * dY0dA0[k + 3, 1] - P_io * dY0dA0[k + 3, 2]; dA0dY0[4, k + 3] = +P_eo * dY0dA0[k, 1] + P_io * dY0dA0[k, 2]; dA0dY0[5, k] = -P_aM * dY0dA0[k + 3, 0] - P_eM * dY0dA0[k + 3, 1]; dA0dY0[5, k + 3] = +P_aM * dY0dA0[k, 0] + P_eM * dY0dA0[k, 1]; } ; // State transition matrix dYdY0 = dYdA0 * dA0dY0; }
public void Integ(double tout, ref Geo.Algorithm.Vector y) { do { Integ_(ref t, tout, ref y); if (State == DE_STATE.DE_INVPARAM) { var info = "ERROR: invalid parameters in public Integ"; throw new ArgumentException(info); // << std::endl; exit(1); } if (State == DE_STATE.DE_BADACC) { var info = "WARNING: Accuracy requirement not achieved in public Integ"; Console.WriteLine(info); } if (State == DE_STATE.DE_STIFF) { var info = "WARNING: Stiff problem suspected in public Integ"; Console.WriteLine(info); } }while (State > DE_STATE.DE_DONE); }
} // Square-root information matrix // (Upper right triangular matrix) /// <summary> ///Constructor LSQ class (implementation) /// </summary> /// <param name="n">Number of estimation parameters</param> public LsqEstimater(int n) { ParamCount = (n); // Allocate storage for R and d and initialize to zero RightHandSide = new Geo.Algorithm.Vector(ParamCount); SquareRoot = new Matrix(ParamCount, ParamCount); }
Matrix P; // Covariance matrix /// <summary> /// Constructor, EKF class (implementation) /// </summary> /// <param name="n_"></param> public ExtendedKalmanFilter(int n_) { n = (n_); // Number of estimation parameters t = (0.0); // Epoch // Allocate storage and initialize to zero x = new Geo.Algorithm.Vector(n); P = new Matrix(n, n); }
// // Time Update // public void TimeUpdate(double t_, // New epoch Geo.Algorithm.Vector x_, // Propagetd state Matrix Phi) // State transition matrix { t = t_; // Next time step x = x_; // Propagated state P = Phi * P * Phi.Transpose(); // Propagated covariance }
// // Interpolation // public Geo.Algorithm.Vector Intrp( double tout, // Desired output point ref Geo.Algorithm.Vector y // Solution vector ) { Interpolate(tout, ref y, ref ypout); // Interpolate and discard interpolated return(ypout); // derivative ypout }
/// <summary> /// 计算由质点万有引力引起的扰动加速度。 Computes the perturbational acceleration due to a point mass /// </summary> /// <param name="satXyz"> r Satellite position vector </param> /// <param name="massXyz"> s Point mass position vector</param> /// <param name="GM"> GM Gravitational coefficient of point mass</param> /// <returns> Acceleration (a=d^2r/dt^2)</returns> public static Geo.Algorithm.Vector AccelerOfPointMass(Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector massXyz, double GM) { //Relative position vector of satellite w.r.t. point mass Geo.Algorithm.Vector d = satXyz - massXyz; // Acceleration return((-GM) * (d / Math.Pow(d.Norm(), 3) + massXyz / Math.Pow(massXyz.Norm(), 3))); }
static void Main0(string[] args) { // Ground station const double lon_Sta = 11.0 * OrbitConsts.RadPerDeg; // [rad] const double lat_Sta = 48.0 * OrbitConsts.RadPerDeg; // [rad] const double alt_h = 0.0e3; // [m] GeoCoord StaGeoCoord = new GeoCoord(lon_Sta, lat_Sta, alt_h); // Geodetic coordinates // Spacecraft orbit double Mjd_Epoch = DateUtil.DateToMjd(1997, 01, 01, 0, 0, 0); // Epoch const double a = 960.0e3 + OrbitConsts.RadiusOfEarth; // Semimajor axis [m] const double e = 0.0; // Eccentricity const double i = 97.0 * OrbitConsts.RadPerDeg; // Inclination [rad] const double Omega = 130.7 * OrbitConsts.RadPerDeg; // RA ascend. node [rad] const double omega = 0.0 * OrbitConsts.RadPerDeg; // Argument of latitude [rad] const double M0 = 0.0 * OrbitConsts.RadPerDeg; // Mean anomaly at epoch [rad] Vector kepElements = new Geo.Algorithm.Vector(a, e, i, Omega, omega, M0); // Keplerian elements // Variables double Mjd_UTC, dt; // Station var R_Sta = StaGeoCoord.ToXyzVector(OrbitConsts.RadiusOfEarth, OrbitConsts.FlatteningOfEarth); // Geocentric position vector Matrix E = StaGeoCoord.ToLocalNez_Matrix(); // Transformation to // local tangent coordinates // Header var info = "Exercise 2-4: Topocentric satellite motion" + "\r\n" + " Date UTC Az El Dist" + "\r\n" + "yyyy/mm/dd hh:mm:ss.sss [deg] [deg] [km]"; Console.WriteLine(info); // Orbit for (int Minute = 6; Minute <= 24; Minute++) { Mjd_UTC = Mjd_Epoch + Minute / 1440.0; // Time dt = (Mjd_UTC - Mjd_Epoch) * 86400.0; // Time since epoch [s] Geo.Algorithm.Vector r = Kepler.State(OrbitConsts.GM_Earth, kepElements, dt).Slice(0, 2); // Inertial position vector Matrix U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UTC)); // Earth rotation var enz = E * (U * r - R_Sta); // Topocentric position vector double Azim = 0, Elev = 0, Dist; GeoCoord.LocalEnzToPolar(enz, out Azim, out Elev, out Dist); // Azimuth, Elevation info = DateUtil.MjdToDateTimeString(Mjd_UTC) + " " + String.Format("{0, 9:F3}", (Azim * OrbitConsts.DegPerRad)) + " " + String.Format("{0, 9:F3}", (Elev * OrbitConsts.DegPerRad)) + " " + String.Format("{0, 9:F3}", (Dist / 1000.0)); Console.WriteLine(info); } ; Console.ReadKey(); }
/// <summary> /// Interpolation /// </summary> /// <param name="xout"></param> /// <param name="yout"></param> /// <param name="ypout"></param> public void Interpolate(double xout, ref Geo.Algorithm.Vector yout, ref Geo.Algorithm.Vector ypout) { // Variables int i, j, ki; double eta, gamma, hi, psijm1; double temp1, term; double[] g = new double[14], rho = new double[14], w = new double[14]; g[1] = 1.0; rho[1] = 1.0; hi = xout - x; ki = kold + 1; // Initialize w[*] for computing g[*] for (i = 1; i <= ki; i++) { temp1 = i; w[i] = 1.0 / temp1; } // Compute g[*] term = 0.0; for (j = 2; j <= ki; j++) { psijm1 = psi[j - 1]; gamma = (hi + term) / psijm1; eta = hi / psijm1; for (i = 1; i <= ki + 1 - j; i++) { w[i] = gamma * w[i] - eta * w[i + 1]; } g[j] = w[1]; rho[j] = gamma * rho[j - 1]; term = psijm1; } ; // Interpolate for the solution yout and for // the derivative of the solution ypout ypout = new Geo.Algorithm.Vector(yout.Dimension); yout = new Geo.Algorithm.Vector(yout.Dimension); for (j = 1; j <= ki; j++) { i = ki + 1 - j; yout = yout + g[i] * phi.Col(i); ypout = ypout + rho[i] * phi.Col(i); } ; yout = yy + hi * yout; // return yy + hi * yout; // Console.WriteLine(yout); }
/// <summary> /// 获取原始值 /// </summary> /// <param name="index"></param> /// <returns></returns> protected Geo.Algorithm.Vector GetRawValue(int index) { Geo.Algorithm.Vector v = new Geo.Algorithm.Vector(Dimension); for (int i = 0; i < Dimension; i++) { v[i] = Inputs[i][index]; } return(v); }
static void Main0(string[] args) { // Constants const double GM = 1.0; // Gravitational coefficient const double e = 0.1; // Eccentricity const double t_end = 20.0; // End time Geo.Algorithm.Vector Kep = new Geo.Algorithm.Vector(1.0, e, 0.0, 0.0, 0.0, 0.0); // (a,e,i,Omega,omega,M) Geo.Algorithm.Vector y_ref = Kepler.State(GM, Kep, t_end); // Reference solution int[] Steps = { 50, 100, 250, 500, 750, 1000, 1500, 2000 }; // Variables // Function call count int iCase; double t, h; // Time and step aboutSize Geo.Algorithm.Vector y = new Geo.Algorithm.Vector(6); // State vector RungeKutta4StepIntegrator Orbit = new RungeKutta4StepIntegrator(f_Kep6D, 6, (Action)nCallsPlus); // Object for integrating the // differential equation // defined by f_Kep6D using the // 4th-order Runge-Kutta method // Header var info = "Exercise 4-1: Runge-Kutta 4th-order integration" + "\r\n" + "\r\n" + " Problem D1 (e=0.1)" + "\r\n" + "\r\n" + " N_fnc Accuracy Digits " + "\r\n"; Console.WriteLine(info); // Loop over test cases for (iCase = 0; iCase < 8; iCase++) { // Step aboutSize h = t_end / Steps[iCase]; // Initial values t = 0.0; y = new Geo.Algorithm.Vector(1.0 - e, 0.0, 0.0, 0.0, Math.Sqrt((1 + e) / (1 - e)), 0.0); nCalls = 0; // Integration from t=t to t=t_end for (int i = 1; i <= Steps[iCase]; i++) { Orbit.Step(ref t, ref y, h); } // Output info = String.Format("{0, 6:D}{1, 14:E4}{2, 9:F}", nCalls, (y - y_ref).Norm(), -Math.Log10((y - y_ref).Norm())); Console.WriteLine(info); } ; Console.ReadKey(); }
} // Covariance matrix public Geo.Algorithm.Vector StdDev() { // Standard deviation Geo.Algorithm.Vector Sigma = new Geo.Algorithm.Vector(n); for (int i = 0; i < n; i++) { Sigma[i] = Math.Sqrt(P[i, i]); } return(Sigma); }
public void Init(double t_, Geo.Algorithm.Vector x_, Geo.Algorithm.Vector sigma) { t = t_; x = x_; //P = new Matrix(n,n); for (int i = 0; i < n; i++) { P[i, i] = sigma[i] * sigma[i]; } }
/// <summary> /// Update of filter parameters /// </summary> /// <param name="t_"></param> /// <param name="x_"></param> /// <param name="Phi"></param> /// <param name="Qdt"></param> public void TimeUpdate(double t_, // New epoch Geo.Algorithm.Vector x_, // Propagetd state Matrix Phi, // State transition matrix Matrix Qdt) // Accumulated process noise { t = t_; // Next time step x = x_; // Propagated state P = Phi * P * Phi.Transpose() + Qdt; // Propagated covariance + noise }
//------------------------------------------------------------------------------ // // Accel // // Purpose: // // Computes the acceleration of an Earth orbiting satellite due to // the Earth's harmonic gravity field up to degree and order 10 // // Input/Output: // // Mjd_UT Modified Julian Date (Universal Time) // r Satellite position vector in the true-of-date system // n,m Gravity model degree and order // <return> Acceleration (a=d^2r/dt^2) in the true-of-date system // //------------------------------------------------------------------------------ static Geo.Algorithm.Vector Accel(double Mjd_UT, Geo.Algorithm.Vector r, int n, int m) { // Earth rotation matrix Matrix U = Matrix.RotateZ3D(IERS.GetGmstRad(Mjd_UT)); // Acceleration due to harmonic gravity field return(Force.AccelerOfHarmonicGraviFiled(r, U, OrbitConsts.GM_Earth, Force.Grav.R_ref, Force.Grav.CS, n, m)); }
/// <summary> /// 计算由太阳光压引起的加速度。 Computes the acceleration due to solar radiation pressure assuming /// the spacecraft surface normal to the Sun direction /// Notes: r, r_sun, Area, mass, P0 and AU must be given in consistent units, /// e.g. m, m^2, kg and N/m^2. /// </summary> /// <param name="satXyz">r Spacecraft position vector </param> /// <param name="sunXyz"> r_Sun Sun position vector </param> /// <param name="Area">Area Cross-section </param> /// <param name="mass"> mass Spacecraft mass</param> /// <param name="solarRadiPressCoef"> coefOfRadiation Solar radiation pressure coefficient</param> /// <param name="solarRadiPressurePerAU">P0 Solar radiation pressure at 1 AU </param> /// <param name="AstroUnit">AU Length of one Astronomical Unit </param> /// <returns>Acceleration (a=d^2r/dt^2)</returns> public static Geo.Algorithm.Vector AccelerOfSolarRadiPressure(Geo.Algorithm.Vector satXyz, Geo.Algorithm.Vector sunXyz, double Area, double mass, double solarRadiPressCoef, double solarRadiPressurePerAU, double AstroUnit) { //Relative position vector of spacecraft w.r.t. Sun Geo.Algorithm.Vector d = satXyz - sunXyz; //Acceleration return(solarRadiPressCoef * (Area / mass) * solarRadiPressurePerAU * (AstroUnit * AstroUnit) * d / Math.Pow(d.Norm(), 3)); }
// Degrees per radian static void Main0(string[] args) { // Position and velocity Geo.Algorithm.Vector r = new Geo.Algorithm.Vector(+10000.0e3, +40000.0e3, -5000.0e3); // [m] Geo.Algorithm.Vector v = new Geo.Algorithm.Vector(-1.500e3, +1.000e3, -0.100e3); // [m/s] // Variables int i; Geo.Algorithm.Vector y = new Geo.Algorithm.Vector(6), Kep = new Geo.Algorithm.Vector(6); // Orbital elements y = r.Stack(v); Kep = Kepler.Elements(OrbitConsts.GM_Earth, y); // Output var info = "Exercise 2-3: Osculating elements"; Console.WriteLine(info); info = "State vector:"; Console.WriteLine(info); info = " Position "; Console.Write(info); for (i = 0; i < 3; i++) { Console.Write(r[i] / 1000.0 + ", "); //Console.WriteLine(info); } ; info = " [km]"; Console.WriteLine(info); info = " Velocity "; Console.Write(info); for (i = 0; i < 3; i++) { Console.Write(v[i] / 1000.0 + ", "); } Console.WriteLine(" [km/s]"); Console.WriteLine(); Console.WriteLine("Orbital elements:"); Console.WriteLine(" Semimajor axis " + String.Format("{0:f3}", (Kep[0] / 1000.0)) + " km"); Console.WriteLine(" Eccentricity " + String.Format("{0:f3}", Kep[1])); Console.WriteLine(" Inclination " + String.Format("{0:f3}", Kep[2] * OrbitConsts.DegPerRad) + " deg"); Console.WriteLine(" RA ascend. node " + String.Format("{0:f3}", Kep[3] * OrbitConsts.DegPerRad) + " deg"); Console.WriteLine(" Arg. of perigee " + String.Format("{0:f3}", Kep[4] * OrbitConsts.DegPerRad) + " deg"); Console.WriteLine(" Mean anomaly " + String.Format("{0:f3}", Kep[5] * OrbitConsts.DegPerRad) + " deg"); Console.ReadKey(); }
/// <summary> /// 计算模糊度,部分模糊度固定策略。 /// </summary> /// <param name="floatSolution">按照Q从小到大的顺序排列后的带权向量。</param> /// <param name="MaxRatio">最大比例</param> /// <returns></returns> public static WeightedVector GetAmbiguity(WeightedVector floatSolution, double MaxRatio = 3, double defaultRms = 1e-20) { //integer least square #region RTKLIB lambda var currentFloat = floatSolution; int i = 0; var lambda = new LambdaAmbiguitySearcher(currentFloat); double[] N1 = new double[currentFloat.Count * 2]; for (i = 0; i < currentFloat.Count; i++) { N1[i] = Math.Round(currentFloat[i]); } double[] s = new double[2]; int info = 0; info = lambda.getLambda(ref N1, ref s); //if (info == -1 || s[0]<=0.0) return 0; //固定失败 double ratio = s[1] / s[0]; while (info == -1 || ratio <= MaxRatio) //检验没有通过 { //去掉一个最大的方差值,继续固定。 var nextParamCount = currentFloat.Count - 1; if (nextParamCount < 1) { return(new WeightedVector()); } currentFloat = currentFloat.GetWeightedVector(0, nextParamCount); //固定 N1 = new double[currentFloat.Count * 2]; for (i = 0; i < currentFloat.Count; i++) { N1[i] = Math.Round(currentFloat[i]); } s = new double[2]; lambda = new LambdaAmbiguitySearcher(currentFloat); info = lambda.getLambda(ref N1, ref s); if (s[0] == 0 && s[1] != 0) // { break; } ratio = s[1] / s[0]; } if (info == -1) { return(new WeightedVector()); } #endregion var integers = ArrayUtil.GetSubArray <double>(N1, 0, currentFloat.Count); Vector Vector = new Geo.Algorithm.Vector(integers, currentFloat.ParamNames); return(new WeightedVector(Vector, defaultRms)); }
/// <summary> /// Computes the second time derivative of the position vector for the /// normalized (GM=1) Kepler's problem in three dimensions. /// pAux is expected to point to an integer variable that will be incremented /// by one on each call of f_Kep3D /// /// </summary> /// <param name="t"></param> /// <param name="r"></param> /// <param name="v"></param> /// <param name="a"></param> /// <param name="pAux"></param> static void f_Kep3D(double t, Geo.Algorithm.Vector r, Geo.Algorithm.Vector v, ref Geo.Algorithm.Vector a, Action pAux) { // Pointer to auxiliary integer variable used as function call counter //int* pCalls = static_cast<int*>(pAux); // 2nd order derivative d^2(r)/dt^2 of the position vector a = new Geo.Algorithm.Vector(-r / (Math.Pow(r.Norm(), 3))); // Increment function call count //(*pCalls)++; pAux(); }
/// <summary> /// 本地空间直角坐标到极坐标的转换,结果包括微分。 AzEl Computes azimuth, elevation and partials from local tangent coordinates /// </summary> /// <param name="localEnz">本地水平坐标ENZ, Topocentric local tangent coordinates (East-North-Zenith frame)</param> /// <param name="azimuthRad"> A Azimuth [rad] </param> /// <param name="elevationRad"> E Elevation [rad]</param> /// <param name="dAds"> dAds Partials of azimuth w.r.t. s</param> /// <param name="dEds"> dEds Partials of elevation w.r.t. s</param> static public void LocalEnzToPolar(Geo.Algorithm.Vector localEnz, out double azimuthRad, out double elevationRad, out Geo.Algorithm.Vector dAds, out Geo.Algorithm.Vector dEds) { double rho = Math.Sqrt(localEnz[0] * localEnz[0] + localEnz[1] * localEnz[1]); // Angles azimuthRad = Math.Atan2(localEnz[0], localEnz[1]); azimuthRad = ((azimuthRad < 0.0) ? azimuthRad + OrbitConsts.TwoPI : azimuthRad); elevationRad = Math.Atan(localEnz[2] / rho); // Partials dAds = new Geo.Algorithm.Vector(localEnz[1] / (rho * rho), -localEnz[0] / (rho * rho), 0.0); dEds = new Geo.Algorithm.Vector(-localEnz[0] * localEnz[2] / rho, -localEnz[1] * localEnz[2] / rho, rho) / localEnz.Dot(localEnz); }
/// <summary> /// Initialization of backwards differences from initial conditions /// </summary> /// <param name="t_0"></param> /// <param name="r_0"></param> /// <param name="v_0"></param> /// <param name="h_"></param> public void Init(double t_0, Geo.Algorithm.Vector r_0, Geo.Algorithm.Vector v_0, double h_) { // Order of method const int m = 4; // Coefficients gamma/delta of 1st/2nd order Moulton/Cowell corrector method double[] gc = { +1.0, -1 / 2.0, -1 / 12.0, -1 / 24.0, -19 / 720.0 }; double[] dc = { +1.0, -1.0, +1 / 12.0, 0.0, -1 / 240.0, -1 / 240.0 }; int i, j; double t = t_0; Geo.Algorithm.Vector r = r_0; Geo.Algorithm.Vector v = v_0; // Save step aboutSize this.StepSize = h_; // Create table of accelerations at past times t-3h, t-2h, and t-h using // RK4 steps DevFunc(t, r, v, ref D[0], pAux); // D[i]=a(t-ih) for (i = 1; i <= m - 1; i++) { RK4(ref t, ref r, ref v, -StepSize); DevFunc(t, r, v, ref D[i], pAux); } ; // Compute backwards differences for (i = 1; i <= m - 1; i++) { for (j = m - 1; j >= i; j--) { D[j] = D[j - 1] - D[j]; } } // Initialize backwards sums using 4th order GJ corrector S1 = v_0 / StepSize; for (i = 1; i <= m; i++) { S1 -= gc[i] * D[i - 1]; } S2 = r_0 / (StepSize * StepSize) - dc[1] * S1; for (i = 2; i <= m + 1; i++) { S2 -= dc[i] * D[i - 2]; } }
/// <summary> /// Integration step /// </summary> /// <param name="t">Value of the independent variable; updated by t+h</param> /// <param name="y">Value of y(t); updated by y(t+h)</param> /// <param name="h">Step aboutSize</param> public void Step(ref double t, ref Geo.Algorithm.Vector y, double h) { // Elementary RK4 step k_1 = DevFunc(t, y, pAux); k_2 = DevFunc(t + h / 2.0, (y + (h / 2.0) * k_1), pAux); k_3 = DevFunc(t + h / 2.0, (y + (h / 2.0) * k_2), pAux); k_4 = DevFunc(t + h, (y + h * k_3), pAux); y = y + (h / 6.0) * (k_1 + 2.0 * k_2 + 2.0 * k_3 + k_4); // Update independent variable t = t + h; }
/// <summary> /// Dyadic product。两个n,m维矩阵向量,点乘为 n × m 矩阵。 /// </summary> /// <param name="left"></param> /// <param name="right"></param> /// <returns></returns> public static Matrix Dyadic(Geo.Algorithm.Vector left, Geo.Algorithm.Vector right) { Matrix Mat = new Matrix(left.Dimension, right.Dimension); for (int i = 0; i < left.Dimension; i++) { for (int j = 0; j < right.Dimension; j++) { Mat.Data[i][j] = left.Data[i] * right.Data[j]; } } return(Mat); }