Telescope Control Library
Diff: CelestialMath.cpp
- Revision:
- 0:6cb2eaf8b133
- Child:
- 18:3ea58b079adc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CelestialMath.cpp Sun Aug 19 05:21:20 2018 +0000 @@ -0,0 +1,1035 @@ +/* + * CelestialMath.cpp + * + * Created on: Feb 21, 2018 + * Author: Yuan + */ + +#include "CelestialMath.h" +#include <math.h> +#include <stdio.h> +#include <stdlib.h> +#include "mbed.h" + +#define CM_DEBUG 0 + +static inline double clamp(double x) +{ + return (x > 1) ? 1 : ((x < -1) ? -1 : x); +} + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define MAX_ITERATION 30 +#define MAX_ITERATION_OPTIMIZATION 10 + +static const double tol = 1e-10; +static const double eps = 1e-13; +static const double delta = 1e-7; + +static const double RADIAN = 180.0 / M_PI; +static const double DEGREE = M_PI / 180.0; + +CartesianVector CartesianVector::operator*(const Transformation &t) +{ + return CartesianVector(t.a11 * x + t.a21 * y + t.a31 * z, + t.a12 * x + t.a22 * y + t.a32 * z, + t.a13 * x + t.a23 * y + t.a33 * z); +} + +CartesianVector Transformation::operator*(const CartesianVector &vec) +{ // Left-product of matrix and vector + return CartesianVector(a11 * vec.x + a12 * vec.y + a13 * vec.z, + a21 * vec.x + a22 * vec.y + a23 * vec.z, + a31 * vec.x + a32 * vec.y + a33 * vec.z); +} + +AzimuthalCoordinates CelestialMath::localEquatorialToAzimuthal( + const LocalEquatorialCoordinates &a, const LocationCoordinates &loc) +{ + // cphi lambda lambda0 + double c1 = cos(a.ha * DEGREE), c2 = cos(a.dec * DEGREE), c3 = cos( + loc.lat * DEGREE); + double s1 = sin(a.ha * DEGREE), s2 = sin(a.dec * DEGREE), s3 = sin( + loc.lat * DEGREE); + double s4 = c1 * c2 * c3 + s2 * s3; + s4 = clamp(s4); + double y1 = s1 * c2, x1 = s2 * c3 - c1 * c2 * s3; + + return AzimuthalCoordinates(asin(clamp(s4)) * RADIAN, + atan2(y1, x1) * RADIAN); +} + +LocalEquatorialCoordinates CelestialMath::azimuthalToLocalEquatorial( + const AzimuthalCoordinates &b, const LocationCoordinates &loc) +{ + // mu eps lambda0 + double c1 = cos(b.azi * DEGREE), c2 = cos(b.alt * DEGREE), c3 = cos( + loc.lat * DEGREE); + double s1 = sin(b.azi * DEGREE), s2 = sin(b.alt * DEGREE), s3 = sin( + loc.lat * DEGREE); + double s4 = c1 * c2 * c3 + s2 * s3; + double y1 = s1 * c2, x1 = s2 * c3 - c1 * c2 * s3; + + return LocalEquatorialCoordinates(asin(clamp(s4)) * RADIAN, + atan2(y1, x1) * RADIAN); +} + +LocalEquatorialCoordinates AlignmentStar::star_ref_local( + const LocationCoordinates &loc) const +{ + return CelestialMath::equatorialToLocalEquatorial(star_ref, timestamp, loc); +} + +double CelestialMath::getGreenwichMeanSiderealTime(time_t timestamp) +{ + double jd = (double) timestamp * 1.1574074074074E-5 + 2440587.5 - 2451545.0; // Julian Date since J2000 +// double jd0 = floor(jd - 0.5) + 0.5; // JD of previous midnight +// double cent = (jd - 2451545.0) / 36525; +// double gmst = 6.697374558 + 0.06570982441908 * (jd0 - 2451545.0) +// + 1.00273790935 * (jd - jd0) * 24 + 0.000026 * cent * cent; +// gmst *= 15.0; + double gmst = 280.46061837 + 360.985647366 * jd; // Greenwich mean sidereal time (angle) + return remainder(gmst, 360.0); +} + +double CelestialMath::getLocalSiderealTime(time_t timestamp, + const LocationCoordinates &loc) +{ + double gmst = getGreenwichMeanSiderealTime(timestamp); + double lst = gmst + loc.lon * 1.00273790935; // Local sidereal time (angle) + return remainder(lst, 360.0); +} + +LocalEquatorialCoordinates CelestialMath::equatorialToLocalEquatorial( + const EquatorialCoordinates &e, time_t timestamp, + const LocationCoordinates &loc) +{ +// From phi to cphi + return LocalEquatorialCoordinates(e.dec, + remainder(getLocalSiderealTime(timestamp, loc) - e.ra, 360.0)); +} + +EquatorialCoordinates CelestialMath::localEquatorialToEquatorial( + const LocalEquatorialCoordinates &a, time_t timestamp, + const LocationCoordinates &loc) +{ +// From cphi to phi + return EquatorialCoordinates(a.dec, + remainder(getLocalSiderealTime(timestamp, loc) - a.ha, 360.0)); +} + +Transformation &CelestialMath::getMisalignedPolarAxisTransformation( + Transformation &t, const AzimuthalCoordinates &p, + const LocationCoordinates &loc) +{ + double c1 = cos(p.azi * DEGREE), c2 = cos(p.alt * DEGREE), c3 = cos( + loc.lat * DEGREE); + double s1 = sin(p.azi * DEGREE), s2 = sin(p.alt * DEGREE), s3 = sin( + loc.lat * DEGREE); +// Matrix to convert from basis vectors in misaligned PA to correct PA + t.a11 = c1 * s2 * s3 + c2 * c3; + t.a12 = -s1 * s3; + t.a13 = -c1 * c2 * s3 + s2 * c3; + t.a21 = s1 * s2; + t.a22 = c1; + t.a23 = -s1 * c2; + t.a31 = -c1 * s2 * c3 + c2 * s3; + t.a32 = s1 * c3; + t.a33 = c1 * c2 * c3 + s2 * s3; + return t; +} + +LocalEquatorialCoordinates CelestialMath::applyMisalignment( + const Transformation &t, const LocalEquatorialCoordinates& a) +{ + double c1 = cos(a.dec * DEGREE), c2 = cos(a.ha * DEGREE); + double s1 = sin(a.dec * DEGREE), s2 = sin(a.ha * DEGREE); + CartesianVector X = CartesianVector(c1 * c2, -c1 * s2, s1) * t; + + return LocalEquatorialCoordinates(asin(clamp(X.z)) * RADIAN, + atan2(-X.y, X.x) * RADIAN); +} + +LocalEquatorialCoordinates CelestialMath::deapplyMisalignment( + const Transformation &t, const LocalEquatorialCoordinates& a) +{ +// the Transformation is ORTHOGONAL, T^-1 = T' + double c1 = cos(a.dec * DEGREE), c2 = cos(a.ha * DEGREE); + double s1 = sin(a.dec * DEGREE), s2 = sin(a.ha * DEGREE); + Transformation tp = t; + tp.transpose(); + CartesianVector X = CartesianVector(c1 * c2, -c1 * s2, s1) * tp; + + return LocalEquatorialCoordinates(asin(clamp(X.z)) * RADIAN, + atan2(-X.y, X.x) * RADIAN); +} + +LocalEquatorialCoordinates CelestialMath::applyMisalignment( + const LocalEquatorialCoordinates& a, const AzimuthalCoordinates& mpa, + const LocationCoordinates& loc) +{ + Transformation tr; + getMisalignedPolarAxisTransformation(tr, mpa, loc); + return applyMisalignment(tr, a); +} + +LocalEquatorialCoordinates CelestialMath::deapplyMisalignment( + const LocalEquatorialCoordinates& a, const AzimuthalCoordinates& mpa, + const LocationCoordinates& loc) +{ + Transformation tr; + getMisalignedPolarAxisTransformation(tr, mpa, loc); + return deapplyMisalignment(tr, a); +} + +LocalEquatorialCoordinates CelestialMath::applyConeError( + const LocalEquatorialCoordinates& a, double cone) +{ + return LocalEquatorialCoordinates( + asin(clamp(sin(a.dec * DEGREE) / cos(cone * DEGREE))) * RADIAN, + a.ha + - asin(clamp(tan(a.dec * DEGREE) * tan(cone * DEGREE))) + * RADIAN); +} + +LocalEquatorialCoordinates CelestialMath::deapplyConeError( + const LocalEquatorialCoordinates& a, double cone) +{ + double lmd = asin(clamp(sin(a.dec * DEGREE) * cos(cone * DEGREE))) * RADIAN; + if (lmd > 90 - eps || lmd < -90 + eps) // This implies cone=0, so we don't do anything + return a; + double phi = a.ha + + asin(clamp(tan(cone * DEGREE) * tan(lmd * DEGREE))) * RADIAN; + return LocalEquatorialCoordinates(lmd, phi); +} + +MountCoordinates CelestialMath::localEquatorialToMount( + const LocalEquatorialCoordinates& a, pierside_t side) +{ + MountCoordinates m; + double ha = a.ha; + if (side == PIER_SIDE_WEST + || (side == PIER_SIDE_AUTO && (ha = remainder(a.ha, 360.0)) > 0)) + { + m.side = PIER_SIDE_WEST; + m.dec_delta = 90.0 - a.dec; // dec_delta > 0 + m.ra_delta = ha - 90.0; + } + else + { + m.side = PIER_SIDE_EAST; + m.dec_delta = a.dec - 90; // dec_delta<0 + m.ra_delta = ha + 90.0; + } + return m; +} + +LocalEquatorialCoordinates CelestialMath::mountToLocalEquatorial( + const MountCoordinates& m) +{ + LocalEquatorialCoordinates a; + if (m.side == PIER_SIDE_WEST + || (m.side == PIER_SIDE_AUTO && (m.dec_delta > 0))) + { + a.ha = m.ra_delta + 90.0; + a.dec = 90.0 - m.dec_delta; + } + else + { + a.ha = m.ra_delta - 90; + a.dec = 90.0 + m.dec_delta; + } + return a; +} + +AzimuthalCoordinates CelestialMath::alignOneStar( + const LocalEquatorialCoordinates &star_ref, + const LocalEquatorialCoordinates &star_meas, + const LocationCoordinates& loc, const AzimuthalCoordinates &pa_start) +{ + AzimuthalCoordinates pa = pa_start; + +// Perform Newton iteration to obtain a better estimation for PA coordinates + + int i = 0; + double diff = 1e10; + Transformation t, t1, t2; + bool diverge = false; + + while (i++ <= MAX_ITERATION && diff > tol) + { + getMisalignedPolarAxisTransformation(t, pa, loc); + getMisalignedPolarAxisTransformation(t1, + AzimuthalCoordinates(pa.alt + delta, pa.azi), loc); + getMisalignedPolarAxisTransformation(t2, + AzimuthalCoordinates(pa.alt, pa.azi + delta), loc); + + LocalEquatorialCoordinates star = applyMisalignment(t, star_ref), + star1 = applyMisalignment(t1, star_ref), star2 = + applyMisalignment(t2, star_ref); + + // Calculate Jacobian matrix. Everything should be divided by delta + double j11 = star1.dec - star.dec, j12 = star2.dec - star.dec; + double j21 = star1.ha - star.ha, j22 = star2.ha - star.ha; + double det = j11 * j22 - j12 * j21; + + if (det == 0) + { + diverge = true; + break; + } + + // Newton's Method + // Everything should be multiplied by delta + double dp1 = -(j22 * (star.dec - star_meas.dec) + - j12 * (star.ha - star_meas.ha)) / det; + double dp2 = -(-j21 * (star.dec - star_meas.dec) + + j11 * (star.ha - star_meas.ha)) / det; + + // Update the coordinates + pa.alt += dp1 * delta; + pa.azi += dp2 * delta; + + diff = sqrt(dp1 * dp1 + dp2 * dp2) * delta; // calculate the difference + debug_if(CM_DEBUG, "Iteration %i, %f\t%f\tdiff=%f\t %e, %e, %e, %e\n", + i, pa.alt, pa.azi, diff, j11, j12, j21, j22); + } + if (diverge) + { + /// Do something + debug_if(CM_DEBUG, "Diverge\n"); + } + debug_if(CM_DEBUG, "Final delta: %.2e\n", diff); + return pa; +} + +IndexOffset CelestialMath::alignOneStarForOffset( + const LocalEquatorialCoordinates& star_ref, + const MountCoordinates& star_meas) +{ +// Convert the reference star to Mount coordinates using the same pier side setting + MountCoordinates star = localEquatorialToMount(star_ref, star_meas.side); + return IndexOffset(star_meas.dec_delta - star.dec_delta, + star_meas.ra_delta - star.ra_delta); +} + +void CelestialMath::alignTwoStars(const LocalEquatorialCoordinates star_ref[], + const LocalEquatorialCoordinates star_meas[], + const LocationCoordinates& loc, AzimuthalCoordinates& pa, + LocalEquatorialCoordinates& offset) +{ + int i = 0; + double diff = 1e10; + Transformation t, t1, t2; + bool diverge = false; + +// First try to get a rough estimate for offset to avoid divergence +// getMisalignedPolarAxisTransformation(t, pa, loc); +// offset = star_meas[0] - applyMisalignment(t, star_ref[0]); + + while (i++ <= MAX_ITERATION && diff > tol) + { + getMisalignedPolarAxisTransformation(t, pa, loc); + getMisalignedPolarAxisTransformation(t1, + AzimuthalCoordinates(pa.alt + delta, pa.azi), loc); + getMisalignedPolarAxisTransformation(t2, + AzimuthalCoordinates(pa.alt, pa.azi + delta), loc); + + // Transform both starts and add offset + LocalEquatorialCoordinates star[2] = + { applyMisalignment(t, star_ref[0]) + offset, applyMisalignment(t, + star_ref[1]) + offset }; + LocalEquatorialCoordinates star1[2] = + { applyMisalignment(t1, star_ref[0]) + offset, applyMisalignment(t1, + star_ref[1]) + offset }; + LocalEquatorialCoordinates star2[2] = + { applyMisalignment(t2, star_ref[0]) + offset, applyMisalignment(t2, + star_ref[1]) + offset }; + + // Calculate Jacobian matrix. Everything should be divided by delta + // The 4x4 matrix has a special structure, it can be blocked as + // J1 I + // J2 I + // Where J1 and J2 are the 2x2 Jacobians as in alignOneStar and I is 2x2 identity matrix + // Calculate J1=J + double j11 = star1[0].dec - star[0].dec, j12 = star2[0].dec + - star[0].dec; + double j21 = star1[0].ha - star[0].ha, j22 = star2[0].ha - star[0].ha; + // Calculate J2=K + double k11 = star1[1].dec - star[1].dec, k12 = star2[1].dec + - star[1].dec; + double k21 = star1[1].ha - star[1].ha, k22 = star2[1].ha - star[1].ha; + double det = (j11 - k11) * (j22 - k22) - (j12 - k12) * (j21 - k21); // det(J) = det(J1-J2) + if (det == 0) + { + diverge = true; + break; + } + + // Calculate invert of J1-J2 + double i11 = (j22 - k22) / det, i12 = -(j12 - k12) / det; + double i21 = -(j21 - k21) / det, i22 = (j11 - k11) / det; + // Calculate J2(J1-J2)^-1 + double l11 = k11 * i11 + k12 * i21, l12 = k11 * i12 + k12 * i22; + double l21 = k21 * i11 + k22 * i21, l22 = k21 * i12 + k22 * i22; + + // Calculate F1, F2, F3, F4 + double f1 = star[0].dec - star_meas[0].dec; + double f2 = star[0].ha - star_meas[0].ha; + double f3 = star[1].dec - star_meas[1].dec; + double f4 = star[1].ha - star_meas[1].ha; + + // Newton's Method - Calculate J^-1 * F + // dp1,2 should be multiplied by delta + double dp1 = i11 * f1 + i12 * f2 - i11 * f3 - i12 * f4; + double dp2 = i21 * f1 + i22 * f2 - i21 * f3 - i22 * f4; + double dp3 = -l11 * f1 - l12 * f2 + (1 + l11) * f3 + l12 * f4; + double dp4 = -l21 * f1 - l22 * f2 + l21 * f3 + (1 + l22) * f4; + + // Update the coordinates + pa.alt += -dp1 * delta; + pa.azi += -dp2 * delta; + offset.dec += -dp3; + offset.ha += -dp4; + + diff = sqrt(f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4); // calculate the difference + debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\tdiff=%f\t %e %e\n", i, + pa.alt, pa.azi, offset.dec, offset.ha, diff, det, + det * (i11 * i22 - i12 * i21)); + } + if (diverge) + { + /// Do something + debug_if(CM_DEBUG, "Diverge\n"); + } + debug_if(CM_DEBUG, "Final delta: %.2e\n", diff); +} + +void CelestialMath::alignTwoStars(const LocalEquatorialCoordinates star_ref[], + const MountCoordinates star_meas[], const LocationCoordinates& loc, + AzimuthalCoordinates& pa, IndexOffset& offset, bool &diverge) +{ +// Initialize the PA and offset + pa.alt = loc.lat; + pa.azi = 0; + offset = IndexOffset(0, 0); + + int i = 0; + double residue = 1e10; + Transformation t, t1, t2; + diverge = false; + + while (i++ <= MAX_ITERATION && residue > tol) + { + getMisalignedPolarAxisTransformation(t, pa, loc); + getMisalignedPolarAxisTransformation(t1, + AzimuthalCoordinates(pa.alt + delta, pa.azi), loc); + getMisalignedPolarAxisTransformation(t2, + AzimuthalCoordinates(pa.alt, pa.azi + delta), loc); + + // Transform both starts and add offset + MountCoordinates star[2] = + { localEquatorialToMount(applyMisalignment(t, star_ref[0]), + star_meas[0].side) + offset, localEquatorialToMount( + applyMisalignment(t, star_ref[1]), star_meas[1].side) + offset }; + MountCoordinates star1[2] = + { localEquatorialToMount(applyMisalignment(t1, star_ref[0]), + star_meas[0].side) + offset, localEquatorialToMount( + applyMisalignment(t1, star_ref[1]), star_meas[1].side) + + offset }; + MountCoordinates star2[2] = + { localEquatorialToMount(applyMisalignment(t2, star_ref[0]), + star_meas[0].side) + offset, localEquatorialToMount( + applyMisalignment(t2, star_ref[1]), star_meas[1].side) + + offset }; + + // Calculate Jacobian matrix. Everything should be divided by delta + // The 4x4 matrix has a special structure, it can be blocked as + // J1 I + // J2 I + // Where J1 and J2 are the 2x2 Jacobians as in alignOneStar and I is 2x2 identity matrix + // Calculate J1=J + double j11 = star1[0].dec_delta - star[0].dec_delta, j12 = + star2[0].dec_delta - star[0].dec_delta; + double j21 = star1[0].ra_delta - star[0].ra_delta, j22 = + star2[0].ra_delta - star[0].ra_delta; + // Calculate J2=K + double k11 = star1[1].dec_delta - star[1].dec_delta, k12 = + star2[1].dec_delta - star[1].dec_delta; + double k21 = star1[1].ra_delta - star[1].ra_delta, k22 = + star2[1].ra_delta - star[1].ra_delta; + double det = (j11 - k11) * (j22 - k22) - (j12 - k12) * (j21 - k21); // det(J) = det(J1-J2) + if (det == 0) + { + diverge = true; + break; + } + + // Calculate invert of J1-J2 + double i11 = (j22 - k22) / det, i12 = -(j12 - k12) / det; + double i21 = -(j21 - k21) / det, i22 = (j11 - k11) / det; + // Calculate J2(J1-J2)^-1 + double l11 = k11 * i11 + k12 * i21, l12 = k11 * i12 + k12 * i22; + double l21 = k21 * i11 + k22 * i21, l22 = k21 * i12 + k22 * i22; + + // Calculate F1, F2, F3, F4 + double f1 = star[0].dec_delta - star_meas[0].dec_delta; + double f2 = star[0].ra_delta - star_meas[0].ra_delta; + double f3 = star[1].dec_delta - star_meas[1].dec_delta; + double f4 = star[1].ra_delta - star_meas[1].ra_delta; + + // Newton's Method - Calculate J^-1 * F + // dp1,2 should be multiplied by delta + double dp1 = i11 * f1 + i12 * f2 - i11 * f3 - i12 * f4; + double dp2 = i21 * f1 + i22 * f2 - i21 * f3 - i22 * f4; + double dp3 = -l11 * f1 - l12 * f2 + (1 + l11) * f3 + l12 * f4; + double dp4 = -l21 * f1 - l22 * f2 + l21 * f3 + (1 + l22) * f4; + + // Update the coordinates + pa.alt += -dp1 * delta; + pa.azi += -dp2 * delta; + offset.dec_off += -dp3; + offset.ra_off += -dp4; + + residue = sqrt(f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4); // calculate the difference + debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\tdiff=%f\t %e %e\n", i, + pa.alt, pa.azi, offset.dec_off, offset.ra_off, residue, det, + det * (i11 * i22 - i12 * i21)); + } + if (diverge) + { + /// Do something + debug_if(CM_DEBUG, "Diverge\n"); + } + debug_if(CM_DEBUG, "Final delta: %.2e\n", residue); +} + +static double jac[20][5]; // can maximally hold 10 stars +static double jacjac[5][5]; // J'J +static double invj[5][5]; + +static void get_corrected_stars(const int N, LocalEquatorialCoordinates stars[], + const LocalEquatorialCoordinates star_ref[], + const LocationCoordinates& loc, const AzimuthalCoordinates& pa, + const LocalEquatorialCoordinates& offset, double cone) +{ + static Transformation t; + CelestialMath::getMisalignedPolarAxisTransformation(t, pa, loc); + for (int i = 0; i < N; i++) + { + stars[i] = CelestialMath::applyConeError( + CelestialMath::applyMisalignment(t, star_ref[i]), cone) + + offset; + } +} + +static void fill_jacobian(const int N, const int j, + LocalEquatorialCoordinates stars0[], + LocalEquatorialCoordinates stars1[], const double &dd) +{ + for (int i = 0; i < N; i++) + { + jac[i * 2][j] = (stars1[i].dec - stars0[i].dec) / dd; + jac[i * 2 + 1][j] = (stars1[i].ha - stars0[i].ha) / dd; + } +} + +static double det33(int a1, int a2, int a3, int b1, int b2, int b3) +{ + return jacjac[a1][b1] * jacjac[a2][b2] * jacjac[a3][b3] + + jacjac[a1][b2] * jacjac[a2][b3] * jacjac[a3][b1] + + jacjac[a1][b3] * jacjac[a2][b1] * jacjac[a3][b2] + - jacjac[a1][b1] * jacjac[a2][b3] * jacjac[a3][b2] + - jacjac[a1][b2] * jacjac[a2][b1] * jacjac[a3][b3] + - jacjac[a1][b3] * jacjac[a2][b2] * jacjac[a3][b1]; +} + +static double det44(int a1, int a2, int a3, int a4, int b1, int b2, int b3, + int b4) +{ + return jacjac[a1][b1] * det33(a2, a3, a4, b2, b3, b4) + - jacjac[a1][b2] * det33(a2, a3, a4, b1, b3, b4) + + jacjac[a1][b3] * det33(a2, a3, a4, b1, b2, b4) + - jacjac[a1][b4] * det33(a2, a3, a4, b1, b2, b3); +} + +static void invert() +{ + invj[0][0] = det44(1, 2, 3, 4, 1, 2, 3, 4); + invj[1][0] = -det44(1, 2, 3, 4, 0, 2, 3, 4); + invj[2][0] = det44(1, 2, 3, 4, 0, 1, 3, 4); + invj[3][0] = -det44(1, 2, 3, 4, 0, 1, 2, 4); + invj[4][0] = det44(1, 2, 3, 4, 0, 1, 2, 3); + + double det55 = invj[0][0] * jacjac[0][0] + invj[1][0] * jacjac[0][1] + + invj[2][0] * jacjac[0][2] + invj[3][0] * jacjac[0][3] + + invj[4][0] * jacjac[0][4]; + double idet55 = 1.0 / det55; + + invj[0][0] *= idet55; + invj[1][0] *= idet55; + invj[2][0] *= idet55; + invj[3][0] *= idet55; + invj[4][0] *= idet55; + + invj[0][1] = -det44(0, 2, 3, 4, 1, 2, 3, 4) * idet55; + invj[1][1] = det44(0, 2, 3, 4, 0, 2, 3, 4) * idet55; + invj[2][1] = -det44(0, 2, 3, 4, 0, 1, 3, 4) * idet55; + invj[3][1] = det44(0, 2, 3, 4, 0, 1, 2, 4) * idet55; + invj[4][1] = -det44(0, 2, 3, 4, 0, 1, 2, 3) * idet55; + + invj[0][2] = det44(0, 1, 3, 4, 1, 2, 3, 4) * idet55; + invj[1][2] = -det44(0, 1, 3, 4, 0, 2, 3, 4) * idet55; + invj[2][2] = det44(0, 1, 3, 4, 0, 1, 3, 4) * idet55; + invj[3][2] = -det44(0, 1, 3, 4, 0, 1, 2, 4) * idet55; + invj[4][2] = det44(0, 1, 3, 4, 0, 1, 2, 3) * idet55; + + invj[0][3] = -det44(0, 1, 2, 4, 1, 2, 3, 4) * idet55; + invj[1][3] = det44(0, 1, 2, 4, 0, 2, 3, 4) * idet55; + invj[2][3] = -det44(0, 1, 2, 4, 0, 1, 3, 4) * idet55; + invj[3][3] = det44(0, 1, 2, 4, 0, 1, 2, 4) * idet55; + invj[4][3] = -det44(0, 1, 2, 4, 0, 1, 2, 3) * idet55; + + invj[0][4] = det44(0, 1, 2, 3, 1, 2, 3, 4) * idet55; + invj[1][4] = -det44(0, 1, 2, 3, 0, 2, 3, 4) * idet55; + invj[2][4] = det44(0, 1, 2, 3, 0, 1, 3, 4) * idet55; + invj[3][4] = -det44(0, 1, 2, 3, 0, 1, 2, 4) * idet55; + invj[4][4] = det44(0, 1, 2, 3, 0, 1, 2, 3) * idet55; +} + +static inline double sqr(double x) +{ + return x * x; +} + +void CelestialMath::alignNStars(const int N, + const LocalEquatorialCoordinates star_ref[], + const LocalEquatorialCoordinates star_meas[], + const LocationCoordinates& loc, AzimuthalCoordinates& pa, + LocalEquatorialCoordinates& offset, double& cone) +{ + if (N == 2) + { + alignTwoStars(star_ref, star_meas, loc, pa, offset); + cone = 0; + return; + } + if (N <= 1) + { + return; + } + +// Assuming the cone error is not huge, we should be fairly close to the local minimum + int i = 0; + double residue = 1e10; + LocalEquatorialCoordinates stars0[N], stars1[N]; + double dp[5]; + double f[20]; + + while (i++ < MAX_ITERATION_OPTIMIZATION && residue > tol) + { + // Calulate Jacobian + get_corrected_stars(N, stars0, star_ref, loc, pa, offset, cone); + /*Vary pa.alt*/ + get_corrected_stars(N, stars1, star_ref, loc, + AzimuthalCoordinates(pa.alt + delta, pa.azi), offset, cone); + fill_jacobian(N, 0, stars0, stars1, delta); + /*Vary pa.azi*/ + get_corrected_stars(N, stars1, star_ref, loc, + AzimuthalCoordinates(pa.alt, pa.azi + delta), offset, cone); + fill_jacobian(N, 1, stars0, stars1, delta); + /*Vary offset.dec*/ + get_corrected_stars(N, stars1, star_ref, loc, pa, + LocalEquatorialCoordinates(offset.dec + delta, offset.ha), + cone); + fill_jacobian(N, 2, stars0, stars1, delta); + /*Vary offset.ha*/ + get_corrected_stars(N, stars1, star_ref, loc, pa, + LocalEquatorialCoordinates(offset.dec, offset.ha + delta), + cone); + fill_jacobian(N, 3, stars0, stars1, delta); + /*Vary cone*/ + get_corrected_stars(N, stars1, star_ref, loc, pa, offset, cone + delta); + fill_jacobian(N, 4, stars0, stars1, delta); + + // The Jacobian is now filled. It is 2*N rows and 5 columns + // Gauss-Newton method: x_n - x_(n-1) = - (J'J)^-1J' * f_(n-1) + // 1. Matrix multiplication + int p, q, r; + + for (p = 0; p < 5; p++) + { + for (q = 0; q < 5; q++) + { + double s = 0; + for (r = 0; r < 2 * N; r++) + s += jac[r][p] * jac[r][q]; + jacjac[p][q] = s; + } + } + + // 2. Matrix inversion + invert(); + + // 3. Calculate f_(n-1) + double newresidue = 0; + for (p = 0; p < N; p++) + { + f[2 * p] = stars0[p].dec - star_meas[p].dec; + f[2 * p + 1] = stars0[p].ha - star_meas[p].ha; + newresidue += sqr(f[2 * p]) + sqr(f[2 * p + 1]); + } + newresidue = sqrt(newresidue); + // 4. Matrix multiplication + for (p = 0; p < 5; p++) + { + double s = 0; + for (q = 0; q < 5; q++) + { + for (r = 0; r < 2 * N; r++) + { + s += invj[p][q] * jac[r][q] * f[r]; + } + } + dp[p] = -s; + } + // 5. Apply the correction + pa.alt += dp[0]; + pa.azi += dp[1]; + offset.dec += dp[2]; + offset.ha += dp[3]; + cone += dp[4]; + + if (newresidue >= residue - tol) + { + debug_if(CM_DEBUG, "Converged.\n"); + break; + } + else + { + residue = newresidue; + } + debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\t%f\tr=%f\n", i, + pa.alt, pa.azi, offset.dec, offset.ha, cone, residue); + } + + debug_if(CM_DEBUG, "Final result: %f\t%f\t%f\t%f\t%f\tr=%f\n", pa.alt, + pa.azi, offset.dec, offset.ha, cone, residue); +} + +static void get_corrected_stars(const int N, MountCoordinates stars[], + const LocalEquatorialCoordinates star_ref[], + const MountCoordinates star_meas[], const LocationCoordinates& loc, + const AzimuthalCoordinates& pa, const IndexOffset& offset, double cone) +{ + static Transformation t; + CelestialMath::getMisalignedPolarAxisTransformation(t, pa, loc); + for (int i = 0; i < N; i++) + { + // Misalign, apply cone error, and transform to mount coordinates using the same pier side as in the measured stars + stars[i] = CelestialMath::localEquatorialToMount( + CelestialMath::applyConeError( + CelestialMath::applyMisalignment(t, star_ref[i]), cone), + star_meas[i].side) + offset; + } +} + +static void fill_jacobian(const int N, const int j, MountCoordinates stars0[], + MountCoordinates stars1[], const double &dd) +{ + for (int i = 0; i < N; i++) + { + jac[i * 2][j] = (stars1[i].dec_delta - stars0[i].dec_delta) / dd; + jac[i * 2 + 1][j] = (stars1[i].ra_delta - stars0[i].ra_delta) / dd; + } +} + +void CelestialMath::alignNStars(const int N, + const LocalEquatorialCoordinates star_ref[], + const MountCoordinates star_meas[], const LocationCoordinates& loc, + AzimuthalCoordinates& pa, IndexOffset& offset, double& cone, + bool &diverge) +{ + if (N == 2) + { + alignTwoStars(star_ref, star_meas, loc, pa, offset, diverge); + cone = 0; + return; + } + if (N <= 1) + { + return; + } + +// Assuming the cone error is not huge, we should be fairly close to the local minimum + int i = 0; + double residue = 1e10; + MountCoordinates stars0[N], stars1[N]; + double dp[5]; + double f[20]; + + diverge = true; + + while (i++ < MAX_ITERATION_OPTIMIZATION && residue > tol) + { + // Calulate Jacobian + get_corrected_stars(N, stars0, star_ref, star_meas, loc, pa, offset, + cone); + /*Vary pa.alt*/ + get_corrected_stars(N, stars1, star_ref, star_meas, loc, + AzimuthalCoordinates(pa.alt + delta, pa.azi), offset, cone); + fill_jacobian(N, 0, stars0, stars1, delta); + /*Vary pa.azi*/ + get_corrected_stars(N, stars1, star_ref, star_meas, loc, + AzimuthalCoordinates(pa.alt, pa.azi + delta), offset, cone); + fill_jacobian(N, 1, stars0, stars1, delta); + /*Vary offset.dec*/ + get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa, + IndexOffset(offset.dec_off + delta, offset.ra_off), cone); + fill_jacobian(N, 2, stars0, stars1, delta); + /*Vary offset.ha*/ + get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa, + IndexOffset(offset.dec_off, offset.ra_off + delta), cone); + fill_jacobian(N, 3, stars0, stars1, delta); + /*Vary cone*/ + get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa, offset, + cone + delta); + fill_jacobian(N, 4, stars0, stars1, delta); + + // The Jacobian is now filled. It is 2*N rows and 5 columns + // Gauss-Newton method: x_n - x_(n-1) = - (J'J)^-1J' * f_(n-1) + // 1. Matrix multiplication + int p, q, r; + + for (p = 0; p < 5; p++) + { + for (q = 0; q < 5; q++) + { + double s = 0; + for (r = 0; r < 2 * N; r++) + s += jac[r][p] * jac[r][q]; + jacjac[p][q] = s; + } + } + + // 2. Matrix inversion + invert(); + + // 3. Calculate f_(n-1) + double newresidue = 0; + for (p = 0; p < N; p++) + { + f[2 * p] = stars0[p].dec_delta - star_meas[p].dec_delta; + f[2 * p + 1] = stars0[p].ra_delta - star_meas[p].ra_delta; + newresidue += sqr(f[2 * p]) + sqr(f[2 * p + 1]); + } + newresidue = sqrt(newresidue); + // 4. Matrix multiplication + for (p = 0; p < 5; p++) + { + double s = 0; + for (q = 0; q < 5; q++) + { + for (r = 0; r < 2 * N; r++) + { + s += invj[p][q] * jac[r][q] * f[r]; + } + } + dp[p] = -s; + } + // 5. Apply the correction + pa.alt += dp[0]; + pa.azi += dp[1]; + offset.dec_off += dp[2]; + offset.ra_off += dp[3]; + cone += dp[4]; + + if (newresidue >= residue - tol) + { + debug_if(CM_DEBUG, "Converged.\n"); + diverge = false; + break; + } + else + { + residue = newresidue; + } + debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\t%f\tr=%f\n", i, + pa.alt, pa.azi, offset.dec_off, offset.ra_off, cone, residue); + } + + if (diverge) + { + debug_if(CM_DEBUG, "Diverged.\n"); + } + + debug_if(CM_DEBUG, "Final result: %f\t%f\t%f\t%f\t%f\tr=%f\n", pa.alt, + pa.azi, offset.dec_off, offset.ra_off, cone, residue); + +} + +EqCalibration CelestialMath::align(const int N, const AlignmentStar stars[], + const LocationCoordinates &loc, bool &diverge) +{ + EqCalibration calib; + calib.pa.alt = loc.lat; + if (N == 1) + { + calib.offset = alignOneStarForOffset(stars[0].star_ref_local(loc), + stars[0].star_meas); + diverge = false; + } + else + { + LocalEquatorialCoordinates star_ref[N]; + MountCoordinates star_meas[N]; + for (int i = 0; i < N; i++) + { + star_ref[i] = stars[i].star_ref_local(loc); + star_meas[i] = stars[i].star_meas; + } + if (N == 2) + { + alignTwoStars(star_ref, star_meas, loc, calib.pa, calib.offset, + diverge); + } + else + { + alignNStars(N, star_ref, star_meas, loc, calib.pa, calib.offset, + calib.cone, diverge); + } + } + calib.error = CelestialMath::alignmentError(N, stars, calib, loc); + + return calib; +} + +double CelestialMath::parseHMSAngle(char* hms) +{ + char *h = strchr(hms, 'h'); + char *m = strchr(hms, 'm'); + char *s = strchr(hms, 's'); + if (h == NULL || m == NULL || s == NULL || !(h < m && m < s)) + { + return NAN; + } + + *h = '\0'; + *m = '\0'; + *s = '\0'; + + char *tp; + int hour = strtol(hms, &tp, 10); + if (tp == hms) + { + return NAN; + } + int minute = strtol(h + 1, &tp, 10); + if (tp == h + 1) + { + return NAN; + } + double second = strtod(m + 1, &tp); + if (tp == m + 1) + { + return NAN; + } + + if (!(hour >= 0 && hour <= 23 && minute >= 0 && minute <= 59 && second >= 0 + && second <= 60)) + { + return NAN; + } + + return remainder((hour + minute / 60.0 + second / 3600.0) * 15, 360); +} + +double CelestialMath::parseDMSAngle(char* dms) +{ + char *d = strchr(dms, 'd'); + char *m = strchr(dms, 'm'); + char *s = strchr(dms, 's'); + if (d == NULL || m == NULL || s == NULL || !(d < m && m < s)) + { + return NAN; + } + + *d = '\0'; + *m = '\0'; + *s = '\0'; + + char *tp; + int degree = strtol(dms, &tp, 10); + if (tp == dms) + { + return NAN; + } + int arcminute = strtol(d + 1, &tp, 10); + if (tp == d + 1) + { + return NAN; + } + double arcsecond = strtod(m + 1, &tp); + if (tp == m + 1) + { + return NAN; + } + + if (!(degree >= -180.0 && degree <= 180.0 && arcminute >= 0 + && arcminute <= 59 && arcsecond >= 0 && arcsecond <= 60)) + { + return NAN; + } + + return remainder((degree + arcminute / 60.0 + arcsecond / 3600.0), 360); +} + +double CelestialMath::kingRate(EquatorialCoordinates eq, + LocationCoordinates loc, time_t time) +{ + LocalEquatorialCoordinates leq = CelestialMath::equatorialToLocalEquatorial( + eq, time, loc); +// AzimuthalCoordinates ac = CelestialMath::localEquatorialToAzimuthal(leq, +// loc); + double cosLat = cos(loc.lat * DEGREE); + double sinLat = sin(loc.lat * DEGREE); + double cotLat = cosLat / sinLat; + double cosDec = cos(eq.dec * DEGREE); + double sinDec = sin(eq.dec * DEGREE); + double tanDec = sinDec / cosDec; + double cosHA = cos(leq.ha * DEGREE); + double kingMpD = (1436.46 + + 0.4 + * (cosLat / cosDec + * (cosLat * cosDec + sinLat * sinDec * cosHA) + / pow(sinLat * sinDec + cosLat * cosDec * cosHA, + 2.0) - cotLat * tanDec * cosHA)); + return 6.0 / kingMpD; +} + +double CelestialMath::alignmentError(const int N, const AlignmentStar stars[], + const EqCalibration &calib, const LocationCoordinates& loc) +{ + static Transformation t; + CelestialMath::getMisalignedPolarAxisTransformation(t, calib.pa, loc); + double r = 0; + for (int i = 0; i < N; i++) + { + // Misalign, apply cone error, and transform to mount coordinates using the same pier side as in the measured stars + MountCoordinates mc = CelestialMath::localEquatorialToMount( + CelestialMath::applyConeError( + CelestialMath::applyMisalignment(t, + CelestialMath::equatorialToLocalEquatorial( + stars[i].star_ref, stars[i].timestamp, + loc)), calib.cone), + stars[i].star_meas.side) + calib.offset; + + r += sqr(mc.ra_delta - stars[i].star_meas.ra_delta) + + sqr(mc.dec_delta - stars[i].star_meas.dec_delta); + } + return sqrt(r); +} +