Telescope Control Library
CelestialMath.cpp@0:6cb2eaf8b133, 2018-08-19 (annotated)
- Committer:
- caoyuan9642
- Date:
- Sun Aug 19 05:21:20 2018 +0000
- Revision:
- 0:6cb2eaf8b133
- Child:
- 18:3ea58b079adc
v0.1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
caoyuan9642 | 0:6cb2eaf8b133 | 1 | /* |
caoyuan9642 | 0:6cb2eaf8b133 | 2 | * CelestialMath.cpp |
caoyuan9642 | 0:6cb2eaf8b133 | 3 | * |
caoyuan9642 | 0:6cb2eaf8b133 | 4 | * Created on: Feb 21, 2018 |
caoyuan9642 | 0:6cb2eaf8b133 | 5 | * Author: Yuan |
caoyuan9642 | 0:6cb2eaf8b133 | 6 | */ |
caoyuan9642 | 0:6cb2eaf8b133 | 7 | |
caoyuan9642 | 0:6cb2eaf8b133 | 8 | #include "CelestialMath.h" |
caoyuan9642 | 0:6cb2eaf8b133 | 9 | #include <math.h> |
caoyuan9642 | 0:6cb2eaf8b133 | 10 | #include <stdio.h> |
caoyuan9642 | 0:6cb2eaf8b133 | 11 | #include <stdlib.h> |
caoyuan9642 | 0:6cb2eaf8b133 | 12 | #include "mbed.h" |
caoyuan9642 | 0:6cb2eaf8b133 | 13 | |
caoyuan9642 | 0:6cb2eaf8b133 | 14 | #define CM_DEBUG 0 |
caoyuan9642 | 0:6cb2eaf8b133 | 15 | |
caoyuan9642 | 0:6cb2eaf8b133 | 16 | static inline double clamp(double x) |
caoyuan9642 | 0:6cb2eaf8b133 | 17 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 18 | return (x > 1) ? 1 : ((x < -1) ? -1 : x); |
caoyuan9642 | 0:6cb2eaf8b133 | 19 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 20 | |
caoyuan9642 | 0:6cb2eaf8b133 | 21 | #ifndef M_PI |
caoyuan9642 | 0:6cb2eaf8b133 | 22 | #define M_PI 3.14159265358979323846 |
caoyuan9642 | 0:6cb2eaf8b133 | 23 | #endif |
caoyuan9642 | 0:6cb2eaf8b133 | 24 | |
caoyuan9642 | 0:6cb2eaf8b133 | 25 | #define MAX_ITERATION 30 |
caoyuan9642 | 0:6cb2eaf8b133 | 26 | #define MAX_ITERATION_OPTIMIZATION 10 |
caoyuan9642 | 0:6cb2eaf8b133 | 27 | |
caoyuan9642 | 0:6cb2eaf8b133 | 28 | static const double tol = 1e-10; |
caoyuan9642 | 0:6cb2eaf8b133 | 29 | static const double eps = 1e-13; |
caoyuan9642 | 0:6cb2eaf8b133 | 30 | static const double delta = 1e-7; |
caoyuan9642 | 0:6cb2eaf8b133 | 31 | |
caoyuan9642 | 0:6cb2eaf8b133 | 32 | static const double RADIAN = 180.0 / M_PI; |
caoyuan9642 | 0:6cb2eaf8b133 | 33 | static const double DEGREE = M_PI / 180.0; |
caoyuan9642 | 0:6cb2eaf8b133 | 34 | |
caoyuan9642 | 0:6cb2eaf8b133 | 35 | CartesianVector CartesianVector::operator*(const Transformation &t) |
caoyuan9642 | 0:6cb2eaf8b133 | 36 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 37 | return CartesianVector(t.a11 * x + t.a21 * y + t.a31 * z, |
caoyuan9642 | 0:6cb2eaf8b133 | 38 | t.a12 * x + t.a22 * y + t.a32 * z, |
caoyuan9642 | 0:6cb2eaf8b133 | 39 | t.a13 * x + t.a23 * y + t.a33 * z); |
caoyuan9642 | 0:6cb2eaf8b133 | 40 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 41 | |
caoyuan9642 | 0:6cb2eaf8b133 | 42 | CartesianVector Transformation::operator*(const CartesianVector &vec) |
caoyuan9642 | 0:6cb2eaf8b133 | 43 | { // Left-product of matrix and vector |
caoyuan9642 | 0:6cb2eaf8b133 | 44 | return CartesianVector(a11 * vec.x + a12 * vec.y + a13 * vec.z, |
caoyuan9642 | 0:6cb2eaf8b133 | 45 | a21 * vec.x + a22 * vec.y + a23 * vec.z, |
caoyuan9642 | 0:6cb2eaf8b133 | 46 | a31 * vec.x + a32 * vec.y + a33 * vec.z); |
caoyuan9642 | 0:6cb2eaf8b133 | 47 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 48 | |
caoyuan9642 | 0:6cb2eaf8b133 | 49 | AzimuthalCoordinates CelestialMath::localEquatorialToAzimuthal( |
caoyuan9642 | 0:6cb2eaf8b133 | 50 | const LocalEquatorialCoordinates &a, const LocationCoordinates &loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 51 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 52 | // cphi lambda lambda0 |
caoyuan9642 | 0:6cb2eaf8b133 | 53 | double c1 = cos(a.ha * DEGREE), c2 = cos(a.dec * DEGREE), c3 = cos( |
caoyuan9642 | 0:6cb2eaf8b133 | 54 | loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 55 | double s1 = sin(a.ha * DEGREE), s2 = sin(a.dec * DEGREE), s3 = sin( |
caoyuan9642 | 0:6cb2eaf8b133 | 56 | loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 57 | double s4 = c1 * c2 * c3 + s2 * s3; |
caoyuan9642 | 0:6cb2eaf8b133 | 58 | s4 = clamp(s4); |
caoyuan9642 | 0:6cb2eaf8b133 | 59 | double y1 = s1 * c2, x1 = s2 * c3 - c1 * c2 * s3; |
caoyuan9642 | 0:6cb2eaf8b133 | 60 | |
caoyuan9642 | 0:6cb2eaf8b133 | 61 | return AzimuthalCoordinates(asin(clamp(s4)) * RADIAN, |
caoyuan9642 | 0:6cb2eaf8b133 | 62 | atan2(y1, x1) * RADIAN); |
caoyuan9642 | 0:6cb2eaf8b133 | 63 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 64 | |
caoyuan9642 | 0:6cb2eaf8b133 | 65 | LocalEquatorialCoordinates CelestialMath::azimuthalToLocalEquatorial( |
caoyuan9642 | 0:6cb2eaf8b133 | 66 | const AzimuthalCoordinates &b, const LocationCoordinates &loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 67 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 68 | // mu eps lambda0 |
caoyuan9642 | 0:6cb2eaf8b133 | 69 | double c1 = cos(b.azi * DEGREE), c2 = cos(b.alt * DEGREE), c3 = cos( |
caoyuan9642 | 0:6cb2eaf8b133 | 70 | loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 71 | double s1 = sin(b.azi * DEGREE), s2 = sin(b.alt * DEGREE), s3 = sin( |
caoyuan9642 | 0:6cb2eaf8b133 | 72 | loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 73 | double s4 = c1 * c2 * c3 + s2 * s3; |
caoyuan9642 | 0:6cb2eaf8b133 | 74 | double y1 = s1 * c2, x1 = s2 * c3 - c1 * c2 * s3; |
caoyuan9642 | 0:6cb2eaf8b133 | 75 | |
caoyuan9642 | 0:6cb2eaf8b133 | 76 | return LocalEquatorialCoordinates(asin(clamp(s4)) * RADIAN, |
caoyuan9642 | 0:6cb2eaf8b133 | 77 | atan2(y1, x1) * RADIAN); |
caoyuan9642 | 0:6cb2eaf8b133 | 78 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 79 | |
caoyuan9642 | 0:6cb2eaf8b133 | 80 | LocalEquatorialCoordinates AlignmentStar::star_ref_local( |
caoyuan9642 | 0:6cb2eaf8b133 | 81 | const LocationCoordinates &loc) const |
caoyuan9642 | 0:6cb2eaf8b133 | 82 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 83 | return CelestialMath::equatorialToLocalEquatorial(star_ref, timestamp, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 84 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 85 | |
caoyuan9642 | 0:6cb2eaf8b133 | 86 | double CelestialMath::getGreenwichMeanSiderealTime(time_t timestamp) |
caoyuan9642 | 0:6cb2eaf8b133 | 87 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 88 | double jd = (double) timestamp * 1.1574074074074E-5 + 2440587.5 - 2451545.0; // Julian Date since J2000 |
caoyuan9642 | 0:6cb2eaf8b133 | 89 | // double jd0 = floor(jd - 0.5) + 0.5; // JD of previous midnight |
caoyuan9642 | 0:6cb2eaf8b133 | 90 | // double cent = (jd - 2451545.0) / 36525; |
caoyuan9642 | 0:6cb2eaf8b133 | 91 | // double gmst = 6.697374558 + 0.06570982441908 * (jd0 - 2451545.0) |
caoyuan9642 | 0:6cb2eaf8b133 | 92 | // + 1.00273790935 * (jd - jd0) * 24 + 0.000026 * cent * cent; |
caoyuan9642 | 0:6cb2eaf8b133 | 93 | // gmst *= 15.0; |
caoyuan9642 | 0:6cb2eaf8b133 | 94 | double gmst = 280.46061837 + 360.985647366 * jd; // Greenwich mean sidereal time (angle) |
caoyuan9642 | 0:6cb2eaf8b133 | 95 | return remainder(gmst, 360.0); |
caoyuan9642 | 0:6cb2eaf8b133 | 96 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 97 | |
caoyuan9642 | 0:6cb2eaf8b133 | 98 | double CelestialMath::getLocalSiderealTime(time_t timestamp, |
caoyuan9642 | 0:6cb2eaf8b133 | 99 | const LocationCoordinates &loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 100 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 101 | double gmst = getGreenwichMeanSiderealTime(timestamp); |
caoyuan9642 | 0:6cb2eaf8b133 | 102 | double lst = gmst + loc.lon * 1.00273790935; // Local sidereal time (angle) |
caoyuan9642 | 0:6cb2eaf8b133 | 103 | return remainder(lst, 360.0); |
caoyuan9642 | 0:6cb2eaf8b133 | 104 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 105 | |
caoyuan9642 | 0:6cb2eaf8b133 | 106 | LocalEquatorialCoordinates CelestialMath::equatorialToLocalEquatorial( |
caoyuan9642 | 0:6cb2eaf8b133 | 107 | const EquatorialCoordinates &e, time_t timestamp, |
caoyuan9642 | 0:6cb2eaf8b133 | 108 | const LocationCoordinates &loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 109 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 110 | // From phi to cphi |
caoyuan9642 | 0:6cb2eaf8b133 | 111 | return LocalEquatorialCoordinates(e.dec, |
caoyuan9642 | 0:6cb2eaf8b133 | 112 | remainder(getLocalSiderealTime(timestamp, loc) - e.ra, 360.0)); |
caoyuan9642 | 0:6cb2eaf8b133 | 113 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 114 | |
caoyuan9642 | 0:6cb2eaf8b133 | 115 | EquatorialCoordinates CelestialMath::localEquatorialToEquatorial( |
caoyuan9642 | 0:6cb2eaf8b133 | 116 | const LocalEquatorialCoordinates &a, time_t timestamp, |
caoyuan9642 | 0:6cb2eaf8b133 | 117 | const LocationCoordinates &loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 118 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 119 | // From cphi to phi |
caoyuan9642 | 0:6cb2eaf8b133 | 120 | return EquatorialCoordinates(a.dec, |
caoyuan9642 | 0:6cb2eaf8b133 | 121 | remainder(getLocalSiderealTime(timestamp, loc) - a.ha, 360.0)); |
caoyuan9642 | 0:6cb2eaf8b133 | 122 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 123 | |
caoyuan9642 | 0:6cb2eaf8b133 | 124 | Transformation &CelestialMath::getMisalignedPolarAxisTransformation( |
caoyuan9642 | 0:6cb2eaf8b133 | 125 | Transformation &t, const AzimuthalCoordinates &p, |
caoyuan9642 | 0:6cb2eaf8b133 | 126 | const LocationCoordinates &loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 127 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 128 | double c1 = cos(p.azi * DEGREE), c2 = cos(p.alt * DEGREE), c3 = cos( |
caoyuan9642 | 0:6cb2eaf8b133 | 129 | loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 130 | double s1 = sin(p.azi * DEGREE), s2 = sin(p.alt * DEGREE), s3 = sin( |
caoyuan9642 | 0:6cb2eaf8b133 | 131 | loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 132 | // Matrix to convert from basis vectors in misaligned PA to correct PA |
caoyuan9642 | 0:6cb2eaf8b133 | 133 | t.a11 = c1 * s2 * s3 + c2 * c3; |
caoyuan9642 | 0:6cb2eaf8b133 | 134 | t.a12 = -s1 * s3; |
caoyuan9642 | 0:6cb2eaf8b133 | 135 | t.a13 = -c1 * c2 * s3 + s2 * c3; |
caoyuan9642 | 0:6cb2eaf8b133 | 136 | t.a21 = s1 * s2; |
caoyuan9642 | 0:6cb2eaf8b133 | 137 | t.a22 = c1; |
caoyuan9642 | 0:6cb2eaf8b133 | 138 | t.a23 = -s1 * c2; |
caoyuan9642 | 0:6cb2eaf8b133 | 139 | t.a31 = -c1 * s2 * c3 + c2 * s3; |
caoyuan9642 | 0:6cb2eaf8b133 | 140 | t.a32 = s1 * c3; |
caoyuan9642 | 0:6cb2eaf8b133 | 141 | t.a33 = c1 * c2 * c3 + s2 * s3; |
caoyuan9642 | 0:6cb2eaf8b133 | 142 | return t; |
caoyuan9642 | 0:6cb2eaf8b133 | 143 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 144 | |
caoyuan9642 | 0:6cb2eaf8b133 | 145 | LocalEquatorialCoordinates CelestialMath::applyMisalignment( |
caoyuan9642 | 0:6cb2eaf8b133 | 146 | const Transformation &t, const LocalEquatorialCoordinates& a) |
caoyuan9642 | 0:6cb2eaf8b133 | 147 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 148 | double c1 = cos(a.dec * DEGREE), c2 = cos(a.ha * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 149 | double s1 = sin(a.dec * DEGREE), s2 = sin(a.ha * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 150 | CartesianVector X = CartesianVector(c1 * c2, -c1 * s2, s1) * t; |
caoyuan9642 | 0:6cb2eaf8b133 | 151 | |
caoyuan9642 | 0:6cb2eaf8b133 | 152 | return LocalEquatorialCoordinates(asin(clamp(X.z)) * RADIAN, |
caoyuan9642 | 0:6cb2eaf8b133 | 153 | atan2(-X.y, X.x) * RADIAN); |
caoyuan9642 | 0:6cb2eaf8b133 | 154 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 155 | |
caoyuan9642 | 0:6cb2eaf8b133 | 156 | LocalEquatorialCoordinates CelestialMath::deapplyMisalignment( |
caoyuan9642 | 0:6cb2eaf8b133 | 157 | const Transformation &t, const LocalEquatorialCoordinates& a) |
caoyuan9642 | 0:6cb2eaf8b133 | 158 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 159 | // the Transformation is ORTHOGONAL, T^-1 = T' |
caoyuan9642 | 0:6cb2eaf8b133 | 160 | double c1 = cos(a.dec * DEGREE), c2 = cos(a.ha * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 161 | double s1 = sin(a.dec * DEGREE), s2 = sin(a.ha * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 162 | Transformation tp = t; |
caoyuan9642 | 0:6cb2eaf8b133 | 163 | tp.transpose(); |
caoyuan9642 | 0:6cb2eaf8b133 | 164 | CartesianVector X = CartesianVector(c1 * c2, -c1 * s2, s1) * tp; |
caoyuan9642 | 0:6cb2eaf8b133 | 165 | |
caoyuan9642 | 0:6cb2eaf8b133 | 166 | return LocalEquatorialCoordinates(asin(clamp(X.z)) * RADIAN, |
caoyuan9642 | 0:6cb2eaf8b133 | 167 | atan2(-X.y, X.x) * RADIAN); |
caoyuan9642 | 0:6cb2eaf8b133 | 168 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 169 | |
caoyuan9642 | 0:6cb2eaf8b133 | 170 | LocalEquatorialCoordinates CelestialMath::applyMisalignment( |
caoyuan9642 | 0:6cb2eaf8b133 | 171 | const LocalEquatorialCoordinates& a, const AzimuthalCoordinates& mpa, |
caoyuan9642 | 0:6cb2eaf8b133 | 172 | const LocationCoordinates& loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 173 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 174 | Transformation tr; |
caoyuan9642 | 0:6cb2eaf8b133 | 175 | getMisalignedPolarAxisTransformation(tr, mpa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 176 | return applyMisalignment(tr, a); |
caoyuan9642 | 0:6cb2eaf8b133 | 177 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 178 | |
caoyuan9642 | 0:6cb2eaf8b133 | 179 | LocalEquatorialCoordinates CelestialMath::deapplyMisalignment( |
caoyuan9642 | 0:6cb2eaf8b133 | 180 | const LocalEquatorialCoordinates& a, const AzimuthalCoordinates& mpa, |
caoyuan9642 | 0:6cb2eaf8b133 | 181 | const LocationCoordinates& loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 182 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 183 | Transformation tr; |
caoyuan9642 | 0:6cb2eaf8b133 | 184 | getMisalignedPolarAxisTransformation(tr, mpa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 185 | return deapplyMisalignment(tr, a); |
caoyuan9642 | 0:6cb2eaf8b133 | 186 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 187 | |
caoyuan9642 | 0:6cb2eaf8b133 | 188 | LocalEquatorialCoordinates CelestialMath::applyConeError( |
caoyuan9642 | 0:6cb2eaf8b133 | 189 | const LocalEquatorialCoordinates& a, double cone) |
caoyuan9642 | 0:6cb2eaf8b133 | 190 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 191 | return LocalEquatorialCoordinates( |
caoyuan9642 | 0:6cb2eaf8b133 | 192 | asin(clamp(sin(a.dec * DEGREE) / cos(cone * DEGREE))) * RADIAN, |
caoyuan9642 | 0:6cb2eaf8b133 | 193 | a.ha |
caoyuan9642 | 0:6cb2eaf8b133 | 194 | - asin(clamp(tan(a.dec * DEGREE) * tan(cone * DEGREE))) |
caoyuan9642 | 0:6cb2eaf8b133 | 195 | * RADIAN); |
caoyuan9642 | 0:6cb2eaf8b133 | 196 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 197 | |
caoyuan9642 | 0:6cb2eaf8b133 | 198 | LocalEquatorialCoordinates CelestialMath::deapplyConeError( |
caoyuan9642 | 0:6cb2eaf8b133 | 199 | const LocalEquatorialCoordinates& a, double cone) |
caoyuan9642 | 0:6cb2eaf8b133 | 200 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 201 | double lmd = asin(clamp(sin(a.dec * DEGREE) * cos(cone * DEGREE))) * RADIAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 202 | if (lmd > 90 - eps || lmd < -90 + eps) // This implies cone=0, so we don't do anything |
caoyuan9642 | 0:6cb2eaf8b133 | 203 | return a; |
caoyuan9642 | 0:6cb2eaf8b133 | 204 | double phi = a.ha |
caoyuan9642 | 0:6cb2eaf8b133 | 205 | + asin(clamp(tan(cone * DEGREE) * tan(lmd * DEGREE))) * RADIAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 206 | return LocalEquatorialCoordinates(lmd, phi); |
caoyuan9642 | 0:6cb2eaf8b133 | 207 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 208 | |
caoyuan9642 | 0:6cb2eaf8b133 | 209 | MountCoordinates CelestialMath::localEquatorialToMount( |
caoyuan9642 | 0:6cb2eaf8b133 | 210 | const LocalEquatorialCoordinates& a, pierside_t side) |
caoyuan9642 | 0:6cb2eaf8b133 | 211 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 212 | MountCoordinates m; |
caoyuan9642 | 0:6cb2eaf8b133 | 213 | double ha = a.ha; |
caoyuan9642 | 0:6cb2eaf8b133 | 214 | if (side == PIER_SIDE_WEST |
caoyuan9642 | 0:6cb2eaf8b133 | 215 | || (side == PIER_SIDE_AUTO && (ha = remainder(a.ha, 360.0)) > 0)) |
caoyuan9642 | 0:6cb2eaf8b133 | 216 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 217 | m.side = PIER_SIDE_WEST; |
caoyuan9642 | 0:6cb2eaf8b133 | 218 | m.dec_delta = 90.0 - a.dec; // dec_delta > 0 |
caoyuan9642 | 0:6cb2eaf8b133 | 219 | m.ra_delta = ha - 90.0; |
caoyuan9642 | 0:6cb2eaf8b133 | 220 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 221 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 222 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 223 | m.side = PIER_SIDE_EAST; |
caoyuan9642 | 0:6cb2eaf8b133 | 224 | m.dec_delta = a.dec - 90; // dec_delta<0 |
caoyuan9642 | 0:6cb2eaf8b133 | 225 | m.ra_delta = ha + 90.0; |
caoyuan9642 | 0:6cb2eaf8b133 | 226 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 227 | return m; |
caoyuan9642 | 0:6cb2eaf8b133 | 228 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 229 | |
caoyuan9642 | 0:6cb2eaf8b133 | 230 | LocalEquatorialCoordinates CelestialMath::mountToLocalEquatorial( |
caoyuan9642 | 0:6cb2eaf8b133 | 231 | const MountCoordinates& m) |
caoyuan9642 | 0:6cb2eaf8b133 | 232 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 233 | LocalEquatorialCoordinates a; |
caoyuan9642 | 0:6cb2eaf8b133 | 234 | if (m.side == PIER_SIDE_WEST |
caoyuan9642 | 0:6cb2eaf8b133 | 235 | || (m.side == PIER_SIDE_AUTO && (m.dec_delta > 0))) |
caoyuan9642 | 0:6cb2eaf8b133 | 236 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 237 | a.ha = m.ra_delta + 90.0; |
caoyuan9642 | 0:6cb2eaf8b133 | 238 | a.dec = 90.0 - m.dec_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 239 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 240 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 241 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 242 | a.ha = m.ra_delta - 90; |
caoyuan9642 | 0:6cb2eaf8b133 | 243 | a.dec = 90.0 + m.dec_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 244 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 245 | return a; |
caoyuan9642 | 0:6cb2eaf8b133 | 246 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 247 | |
caoyuan9642 | 0:6cb2eaf8b133 | 248 | AzimuthalCoordinates CelestialMath::alignOneStar( |
caoyuan9642 | 0:6cb2eaf8b133 | 249 | const LocalEquatorialCoordinates &star_ref, |
caoyuan9642 | 0:6cb2eaf8b133 | 250 | const LocalEquatorialCoordinates &star_meas, |
caoyuan9642 | 0:6cb2eaf8b133 | 251 | const LocationCoordinates& loc, const AzimuthalCoordinates &pa_start) |
caoyuan9642 | 0:6cb2eaf8b133 | 252 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 253 | AzimuthalCoordinates pa = pa_start; |
caoyuan9642 | 0:6cb2eaf8b133 | 254 | |
caoyuan9642 | 0:6cb2eaf8b133 | 255 | // Perform Newton iteration to obtain a better estimation for PA coordinates |
caoyuan9642 | 0:6cb2eaf8b133 | 256 | |
caoyuan9642 | 0:6cb2eaf8b133 | 257 | int i = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 258 | double diff = 1e10; |
caoyuan9642 | 0:6cb2eaf8b133 | 259 | Transformation t, t1, t2; |
caoyuan9642 | 0:6cb2eaf8b133 | 260 | bool diverge = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 261 | |
caoyuan9642 | 0:6cb2eaf8b133 | 262 | while (i++ <= MAX_ITERATION && diff > tol) |
caoyuan9642 | 0:6cb2eaf8b133 | 263 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 264 | getMisalignedPolarAxisTransformation(t, pa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 265 | getMisalignedPolarAxisTransformation(t1, |
caoyuan9642 | 0:6cb2eaf8b133 | 266 | AzimuthalCoordinates(pa.alt + delta, pa.azi), loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 267 | getMisalignedPolarAxisTransformation(t2, |
caoyuan9642 | 0:6cb2eaf8b133 | 268 | AzimuthalCoordinates(pa.alt, pa.azi + delta), loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 269 | |
caoyuan9642 | 0:6cb2eaf8b133 | 270 | LocalEquatorialCoordinates star = applyMisalignment(t, star_ref), |
caoyuan9642 | 0:6cb2eaf8b133 | 271 | star1 = applyMisalignment(t1, star_ref), star2 = |
caoyuan9642 | 0:6cb2eaf8b133 | 272 | applyMisalignment(t2, star_ref); |
caoyuan9642 | 0:6cb2eaf8b133 | 273 | |
caoyuan9642 | 0:6cb2eaf8b133 | 274 | // Calculate Jacobian matrix. Everything should be divided by delta |
caoyuan9642 | 0:6cb2eaf8b133 | 275 | double j11 = star1.dec - star.dec, j12 = star2.dec - star.dec; |
caoyuan9642 | 0:6cb2eaf8b133 | 276 | double j21 = star1.ha - star.ha, j22 = star2.ha - star.ha; |
caoyuan9642 | 0:6cb2eaf8b133 | 277 | double det = j11 * j22 - j12 * j21; |
caoyuan9642 | 0:6cb2eaf8b133 | 278 | |
caoyuan9642 | 0:6cb2eaf8b133 | 279 | if (det == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 280 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 281 | diverge = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 282 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 283 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 284 | |
caoyuan9642 | 0:6cb2eaf8b133 | 285 | // Newton's Method |
caoyuan9642 | 0:6cb2eaf8b133 | 286 | // Everything should be multiplied by delta |
caoyuan9642 | 0:6cb2eaf8b133 | 287 | double dp1 = -(j22 * (star.dec - star_meas.dec) |
caoyuan9642 | 0:6cb2eaf8b133 | 288 | - j12 * (star.ha - star_meas.ha)) / det; |
caoyuan9642 | 0:6cb2eaf8b133 | 289 | double dp2 = -(-j21 * (star.dec - star_meas.dec) |
caoyuan9642 | 0:6cb2eaf8b133 | 290 | + j11 * (star.ha - star_meas.ha)) / det; |
caoyuan9642 | 0:6cb2eaf8b133 | 291 | |
caoyuan9642 | 0:6cb2eaf8b133 | 292 | // Update the coordinates |
caoyuan9642 | 0:6cb2eaf8b133 | 293 | pa.alt += dp1 * delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 294 | pa.azi += dp2 * delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 295 | |
caoyuan9642 | 0:6cb2eaf8b133 | 296 | diff = sqrt(dp1 * dp1 + dp2 * dp2) * delta; // calculate the difference |
caoyuan9642 | 0:6cb2eaf8b133 | 297 | debug_if(CM_DEBUG, "Iteration %i, %f\t%f\tdiff=%f\t %e, %e, %e, %e\n", |
caoyuan9642 | 0:6cb2eaf8b133 | 298 | i, pa.alt, pa.azi, diff, j11, j12, j21, j22); |
caoyuan9642 | 0:6cb2eaf8b133 | 299 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 300 | if (diverge) |
caoyuan9642 | 0:6cb2eaf8b133 | 301 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 302 | /// Do something |
caoyuan9642 | 0:6cb2eaf8b133 | 303 | debug_if(CM_DEBUG, "Diverge\n"); |
caoyuan9642 | 0:6cb2eaf8b133 | 304 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 305 | debug_if(CM_DEBUG, "Final delta: %.2e\n", diff); |
caoyuan9642 | 0:6cb2eaf8b133 | 306 | return pa; |
caoyuan9642 | 0:6cb2eaf8b133 | 307 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 308 | |
caoyuan9642 | 0:6cb2eaf8b133 | 309 | IndexOffset CelestialMath::alignOneStarForOffset( |
caoyuan9642 | 0:6cb2eaf8b133 | 310 | const LocalEquatorialCoordinates& star_ref, |
caoyuan9642 | 0:6cb2eaf8b133 | 311 | const MountCoordinates& star_meas) |
caoyuan9642 | 0:6cb2eaf8b133 | 312 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 313 | // Convert the reference star to Mount coordinates using the same pier side setting |
caoyuan9642 | 0:6cb2eaf8b133 | 314 | MountCoordinates star = localEquatorialToMount(star_ref, star_meas.side); |
caoyuan9642 | 0:6cb2eaf8b133 | 315 | return IndexOffset(star_meas.dec_delta - star.dec_delta, |
caoyuan9642 | 0:6cb2eaf8b133 | 316 | star_meas.ra_delta - star.ra_delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 317 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 318 | |
caoyuan9642 | 0:6cb2eaf8b133 | 319 | void CelestialMath::alignTwoStars(const LocalEquatorialCoordinates star_ref[], |
caoyuan9642 | 0:6cb2eaf8b133 | 320 | const LocalEquatorialCoordinates star_meas[], |
caoyuan9642 | 0:6cb2eaf8b133 | 321 | const LocationCoordinates& loc, AzimuthalCoordinates& pa, |
caoyuan9642 | 0:6cb2eaf8b133 | 322 | LocalEquatorialCoordinates& offset) |
caoyuan9642 | 0:6cb2eaf8b133 | 323 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 324 | int i = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 325 | double diff = 1e10; |
caoyuan9642 | 0:6cb2eaf8b133 | 326 | Transformation t, t1, t2; |
caoyuan9642 | 0:6cb2eaf8b133 | 327 | bool diverge = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 328 | |
caoyuan9642 | 0:6cb2eaf8b133 | 329 | // First try to get a rough estimate for offset to avoid divergence |
caoyuan9642 | 0:6cb2eaf8b133 | 330 | // getMisalignedPolarAxisTransformation(t, pa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 331 | // offset = star_meas[0] - applyMisalignment(t, star_ref[0]); |
caoyuan9642 | 0:6cb2eaf8b133 | 332 | |
caoyuan9642 | 0:6cb2eaf8b133 | 333 | while (i++ <= MAX_ITERATION && diff > tol) |
caoyuan9642 | 0:6cb2eaf8b133 | 334 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 335 | getMisalignedPolarAxisTransformation(t, pa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 336 | getMisalignedPolarAxisTransformation(t1, |
caoyuan9642 | 0:6cb2eaf8b133 | 337 | AzimuthalCoordinates(pa.alt + delta, pa.azi), loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 338 | getMisalignedPolarAxisTransformation(t2, |
caoyuan9642 | 0:6cb2eaf8b133 | 339 | AzimuthalCoordinates(pa.alt, pa.azi + delta), loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 340 | |
caoyuan9642 | 0:6cb2eaf8b133 | 341 | // Transform both starts and add offset |
caoyuan9642 | 0:6cb2eaf8b133 | 342 | LocalEquatorialCoordinates star[2] = |
caoyuan9642 | 0:6cb2eaf8b133 | 343 | { applyMisalignment(t, star_ref[0]) + offset, applyMisalignment(t, |
caoyuan9642 | 0:6cb2eaf8b133 | 344 | star_ref[1]) + offset }; |
caoyuan9642 | 0:6cb2eaf8b133 | 345 | LocalEquatorialCoordinates star1[2] = |
caoyuan9642 | 0:6cb2eaf8b133 | 346 | { applyMisalignment(t1, star_ref[0]) + offset, applyMisalignment(t1, |
caoyuan9642 | 0:6cb2eaf8b133 | 347 | star_ref[1]) + offset }; |
caoyuan9642 | 0:6cb2eaf8b133 | 348 | LocalEquatorialCoordinates star2[2] = |
caoyuan9642 | 0:6cb2eaf8b133 | 349 | { applyMisalignment(t2, star_ref[0]) + offset, applyMisalignment(t2, |
caoyuan9642 | 0:6cb2eaf8b133 | 350 | star_ref[1]) + offset }; |
caoyuan9642 | 0:6cb2eaf8b133 | 351 | |
caoyuan9642 | 0:6cb2eaf8b133 | 352 | // Calculate Jacobian matrix. Everything should be divided by delta |
caoyuan9642 | 0:6cb2eaf8b133 | 353 | // The 4x4 matrix has a special structure, it can be blocked as |
caoyuan9642 | 0:6cb2eaf8b133 | 354 | // J1 I |
caoyuan9642 | 0:6cb2eaf8b133 | 355 | // J2 I |
caoyuan9642 | 0:6cb2eaf8b133 | 356 | // Where J1 and J2 are the 2x2 Jacobians as in alignOneStar and I is 2x2 identity matrix |
caoyuan9642 | 0:6cb2eaf8b133 | 357 | // Calculate J1=J |
caoyuan9642 | 0:6cb2eaf8b133 | 358 | double j11 = star1[0].dec - star[0].dec, j12 = star2[0].dec |
caoyuan9642 | 0:6cb2eaf8b133 | 359 | - star[0].dec; |
caoyuan9642 | 0:6cb2eaf8b133 | 360 | double j21 = star1[0].ha - star[0].ha, j22 = star2[0].ha - star[0].ha; |
caoyuan9642 | 0:6cb2eaf8b133 | 361 | // Calculate J2=K |
caoyuan9642 | 0:6cb2eaf8b133 | 362 | double k11 = star1[1].dec - star[1].dec, k12 = star2[1].dec |
caoyuan9642 | 0:6cb2eaf8b133 | 363 | - star[1].dec; |
caoyuan9642 | 0:6cb2eaf8b133 | 364 | double k21 = star1[1].ha - star[1].ha, k22 = star2[1].ha - star[1].ha; |
caoyuan9642 | 0:6cb2eaf8b133 | 365 | double det = (j11 - k11) * (j22 - k22) - (j12 - k12) * (j21 - k21); // det(J) = det(J1-J2) |
caoyuan9642 | 0:6cb2eaf8b133 | 366 | if (det == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 367 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 368 | diverge = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 369 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 370 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 371 | |
caoyuan9642 | 0:6cb2eaf8b133 | 372 | // Calculate invert of J1-J2 |
caoyuan9642 | 0:6cb2eaf8b133 | 373 | double i11 = (j22 - k22) / det, i12 = -(j12 - k12) / det; |
caoyuan9642 | 0:6cb2eaf8b133 | 374 | double i21 = -(j21 - k21) / det, i22 = (j11 - k11) / det; |
caoyuan9642 | 0:6cb2eaf8b133 | 375 | // Calculate J2(J1-J2)^-1 |
caoyuan9642 | 0:6cb2eaf8b133 | 376 | double l11 = k11 * i11 + k12 * i21, l12 = k11 * i12 + k12 * i22; |
caoyuan9642 | 0:6cb2eaf8b133 | 377 | double l21 = k21 * i11 + k22 * i21, l22 = k21 * i12 + k22 * i22; |
caoyuan9642 | 0:6cb2eaf8b133 | 378 | |
caoyuan9642 | 0:6cb2eaf8b133 | 379 | // Calculate F1, F2, F3, F4 |
caoyuan9642 | 0:6cb2eaf8b133 | 380 | double f1 = star[0].dec - star_meas[0].dec; |
caoyuan9642 | 0:6cb2eaf8b133 | 381 | double f2 = star[0].ha - star_meas[0].ha; |
caoyuan9642 | 0:6cb2eaf8b133 | 382 | double f3 = star[1].dec - star_meas[1].dec; |
caoyuan9642 | 0:6cb2eaf8b133 | 383 | double f4 = star[1].ha - star_meas[1].ha; |
caoyuan9642 | 0:6cb2eaf8b133 | 384 | |
caoyuan9642 | 0:6cb2eaf8b133 | 385 | // Newton's Method - Calculate J^-1 * F |
caoyuan9642 | 0:6cb2eaf8b133 | 386 | // dp1,2 should be multiplied by delta |
caoyuan9642 | 0:6cb2eaf8b133 | 387 | double dp1 = i11 * f1 + i12 * f2 - i11 * f3 - i12 * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 388 | double dp2 = i21 * f1 + i22 * f2 - i21 * f3 - i22 * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 389 | double dp3 = -l11 * f1 - l12 * f2 + (1 + l11) * f3 + l12 * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 390 | double dp4 = -l21 * f1 - l22 * f2 + l21 * f3 + (1 + l22) * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 391 | |
caoyuan9642 | 0:6cb2eaf8b133 | 392 | // Update the coordinates |
caoyuan9642 | 0:6cb2eaf8b133 | 393 | pa.alt += -dp1 * delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 394 | pa.azi += -dp2 * delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 395 | offset.dec += -dp3; |
caoyuan9642 | 0:6cb2eaf8b133 | 396 | offset.ha += -dp4; |
caoyuan9642 | 0:6cb2eaf8b133 | 397 | |
caoyuan9642 | 0:6cb2eaf8b133 | 398 | diff = sqrt(f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4); // calculate the difference |
caoyuan9642 | 0:6cb2eaf8b133 | 399 | debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\tdiff=%f\t %e %e\n", i, |
caoyuan9642 | 0:6cb2eaf8b133 | 400 | pa.alt, pa.azi, offset.dec, offset.ha, diff, det, |
caoyuan9642 | 0:6cb2eaf8b133 | 401 | det * (i11 * i22 - i12 * i21)); |
caoyuan9642 | 0:6cb2eaf8b133 | 402 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 403 | if (diverge) |
caoyuan9642 | 0:6cb2eaf8b133 | 404 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 405 | /// Do something |
caoyuan9642 | 0:6cb2eaf8b133 | 406 | debug_if(CM_DEBUG, "Diverge\n"); |
caoyuan9642 | 0:6cb2eaf8b133 | 407 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 408 | debug_if(CM_DEBUG, "Final delta: %.2e\n", diff); |
caoyuan9642 | 0:6cb2eaf8b133 | 409 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 410 | |
caoyuan9642 | 0:6cb2eaf8b133 | 411 | void CelestialMath::alignTwoStars(const LocalEquatorialCoordinates star_ref[], |
caoyuan9642 | 0:6cb2eaf8b133 | 412 | const MountCoordinates star_meas[], const LocationCoordinates& loc, |
caoyuan9642 | 0:6cb2eaf8b133 | 413 | AzimuthalCoordinates& pa, IndexOffset& offset, bool &diverge) |
caoyuan9642 | 0:6cb2eaf8b133 | 414 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 415 | // Initialize the PA and offset |
caoyuan9642 | 0:6cb2eaf8b133 | 416 | pa.alt = loc.lat; |
caoyuan9642 | 0:6cb2eaf8b133 | 417 | pa.azi = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 418 | offset = IndexOffset(0, 0); |
caoyuan9642 | 0:6cb2eaf8b133 | 419 | |
caoyuan9642 | 0:6cb2eaf8b133 | 420 | int i = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 421 | double residue = 1e10; |
caoyuan9642 | 0:6cb2eaf8b133 | 422 | Transformation t, t1, t2; |
caoyuan9642 | 0:6cb2eaf8b133 | 423 | diverge = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 424 | |
caoyuan9642 | 0:6cb2eaf8b133 | 425 | while (i++ <= MAX_ITERATION && residue > tol) |
caoyuan9642 | 0:6cb2eaf8b133 | 426 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 427 | getMisalignedPolarAxisTransformation(t, pa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 428 | getMisalignedPolarAxisTransformation(t1, |
caoyuan9642 | 0:6cb2eaf8b133 | 429 | AzimuthalCoordinates(pa.alt + delta, pa.azi), loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 430 | getMisalignedPolarAxisTransformation(t2, |
caoyuan9642 | 0:6cb2eaf8b133 | 431 | AzimuthalCoordinates(pa.alt, pa.azi + delta), loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 432 | |
caoyuan9642 | 0:6cb2eaf8b133 | 433 | // Transform both starts and add offset |
caoyuan9642 | 0:6cb2eaf8b133 | 434 | MountCoordinates star[2] = |
caoyuan9642 | 0:6cb2eaf8b133 | 435 | { localEquatorialToMount(applyMisalignment(t, star_ref[0]), |
caoyuan9642 | 0:6cb2eaf8b133 | 436 | star_meas[0].side) + offset, localEquatorialToMount( |
caoyuan9642 | 0:6cb2eaf8b133 | 437 | applyMisalignment(t, star_ref[1]), star_meas[1].side) + offset }; |
caoyuan9642 | 0:6cb2eaf8b133 | 438 | MountCoordinates star1[2] = |
caoyuan9642 | 0:6cb2eaf8b133 | 439 | { localEquatorialToMount(applyMisalignment(t1, star_ref[0]), |
caoyuan9642 | 0:6cb2eaf8b133 | 440 | star_meas[0].side) + offset, localEquatorialToMount( |
caoyuan9642 | 0:6cb2eaf8b133 | 441 | applyMisalignment(t1, star_ref[1]), star_meas[1].side) |
caoyuan9642 | 0:6cb2eaf8b133 | 442 | + offset }; |
caoyuan9642 | 0:6cb2eaf8b133 | 443 | MountCoordinates star2[2] = |
caoyuan9642 | 0:6cb2eaf8b133 | 444 | { localEquatorialToMount(applyMisalignment(t2, star_ref[0]), |
caoyuan9642 | 0:6cb2eaf8b133 | 445 | star_meas[0].side) + offset, localEquatorialToMount( |
caoyuan9642 | 0:6cb2eaf8b133 | 446 | applyMisalignment(t2, star_ref[1]), star_meas[1].side) |
caoyuan9642 | 0:6cb2eaf8b133 | 447 | + offset }; |
caoyuan9642 | 0:6cb2eaf8b133 | 448 | |
caoyuan9642 | 0:6cb2eaf8b133 | 449 | // Calculate Jacobian matrix. Everything should be divided by delta |
caoyuan9642 | 0:6cb2eaf8b133 | 450 | // The 4x4 matrix has a special structure, it can be blocked as |
caoyuan9642 | 0:6cb2eaf8b133 | 451 | // J1 I |
caoyuan9642 | 0:6cb2eaf8b133 | 452 | // J2 I |
caoyuan9642 | 0:6cb2eaf8b133 | 453 | // Where J1 and J2 are the 2x2 Jacobians as in alignOneStar and I is 2x2 identity matrix |
caoyuan9642 | 0:6cb2eaf8b133 | 454 | // Calculate J1=J |
caoyuan9642 | 0:6cb2eaf8b133 | 455 | double j11 = star1[0].dec_delta - star[0].dec_delta, j12 = |
caoyuan9642 | 0:6cb2eaf8b133 | 456 | star2[0].dec_delta - star[0].dec_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 457 | double j21 = star1[0].ra_delta - star[0].ra_delta, j22 = |
caoyuan9642 | 0:6cb2eaf8b133 | 458 | star2[0].ra_delta - star[0].ra_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 459 | // Calculate J2=K |
caoyuan9642 | 0:6cb2eaf8b133 | 460 | double k11 = star1[1].dec_delta - star[1].dec_delta, k12 = |
caoyuan9642 | 0:6cb2eaf8b133 | 461 | star2[1].dec_delta - star[1].dec_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 462 | double k21 = star1[1].ra_delta - star[1].ra_delta, k22 = |
caoyuan9642 | 0:6cb2eaf8b133 | 463 | star2[1].ra_delta - star[1].ra_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 464 | double det = (j11 - k11) * (j22 - k22) - (j12 - k12) * (j21 - k21); // det(J) = det(J1-J2) |
caoyuan9642 | 0:6cb2eaf8b133 | 465 | if (det == 0) |
caoyuan9642 | 0:6cb2eaf8b133 | 466 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 467 | diverge = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 468 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 469 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 470 | |
caoyuan9642 | 0:6cb2eaf8b133 | 471 | // Calculate invert of J1-J2 |
caoyuan9642 | 0:6cb2eaf8b133 | 472 | double i11 = (j22 - k22) / det, i12 = -(j12 - k12) / det; |
caoyuan9642 | 0:6cb2eaf8b133 | 473 | double i21 = -(j21 - k21) / det, i22 = (j11 - k11) / det; |
caoyuan9642 | 0:6cb2eaf8b133 | 474 | // Calculate J2(J1-J2)^-1 |
caoyuan9642 | 0:6cb2eaf8b133 | 475 | double l11 = k11 * i11 + k12 * i21, l12 = k11 * i12 + k12 * i22; |
caoyuan9642 | 0:6cb2eaf8b133 | 476 | double l21 = k21 * i11 + k22 * i21, l22 = k21 * i12 + k22 * i22; |
caoyuan9642 | 0:6cb2eaf8b133 | 477 | |
caoyuan9642 | 0:6cb2eaf8b133 | 478 | // Calculate F1, F2, F3, F4 |
caoyuan9642 | 0:6cb2eaf8b133 | 479 | double f1 = star[0].dec_delta - star_meas[0].dec_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 480 | double f2 = star[0].ra_delta - star_meas[0].ra_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 481 | double f3 = star[1].dec_delta - star_meas[1].dec_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 482 | double f4 = star[1].ra_delta - star_meas[1].ra_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 483 | |
caoyuan9642 | 0:6cb2eaf8b133 | 484 | // Newton's Method - Calculate J^-1 * F |
caoyuan9642 | 0:6cb2eaf8b133 | 485 | // dp1,2 should be multiplied by delta |
caoyuan9642 | 0:6cb2eaf8b133 | 486 | double dp1 = i11 * f1 + i12 * f2 - i11 * f3 - i12 * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 487 | double dp2 = i21 * f1 + i22 * f2 - i21 * f3 - i22 * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 488 | double dp3 = -l11 * f1 - l12 * f2 + (1 + l11) * f3 + l12 * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 489 | double dp4 = -l21 * f1 - l22 * f2 + l21 * f3 + (1 + l22) * f4; |
caoyuan9642 | 0:6cb2eaf8b133 | 490 | |
caoyuan9642 | 0:6cb2eaf8b133 | 491 | // Update the coordinates |
caoyuan9642 | 0:6cb2eaf8b133 | 492 | pa.alt += -dp1 * delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 493 | pa.azi += -dp2 * delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 494 | offset.dec_off += -dp3; |
caoyuan9642 | 0:6cb2eaf8b133 | 495 | offset.ra_off += -dp4; |
caoyuan9642 | 0:6cb2eaf8b133 | 496 | |
caoyuan9642 | 0:6cb2eaf8b133 | 497 | residue = sqrt(f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4); // calculate the difference |
caoyuan9642 | 0:6cb2eaf8b133 | 498 | debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\tdiff=%f\t %e %e\n", i, |
caoyuan9642 | 0:6cb2eaf8b133 | 499 | pa.alt, pa.azi, offset.dec_off, offset.ra_off, residue, det, |
caoyuan9642 | 0:6cb2eaf8b133 | 500 | det * (i11 * i22 - i12 * i21)); |
caoyuan9642 | 0:6cb2eaf8b133 | 501 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 502 | if (diverge) |
caoyuan9642 | 0:6cb2eaf8b133 | 503 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 504 | /// Do something |
caoyuan9642 | 0:6cb2eaf8b133 | 505 | debug_if(CM_DEBUG, "Diverge\n"); |
caoyuan9642 | 0:6cb2eaf8b133 | 506 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 507 | debug_if(CM_DEBUG, "Final delta: %.2e\n", residue); |
caoyuan9642 | 0:6cb2eaf8b133 | 508 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 509 | |
caoyuan9642 | 0:6cb2eaf8b133 | 510 | static double jac[20][5]; // can maximally hold 10 stars |
caoyuan9642 | 0:6cb2eaf8b133 | 511 | static double jacjac[5][5]; // J'J |
caoyuan9642 | 0:6cb2eaf8b133 | 512 | static double invj[5][5]; |
caoyuan9642 | 0:6cb2eaf8b133 | 513 | |
caoyuan9642 | 0:6cb2eaf8b133 | 514 | static void get_corrected_stars(const int N, LocalEquatorialCoordinates stars[], |
caoyuan9642 | 0:6cb2eaf8b133 | 515 | const LocalEquatorialCoordinates star_ref[], |
caoyuan9642 | 0:6cb2eaf8b133 | 516 | const LocationCoordinates& loc, const AzimuthalCoordinates& pa, |
caoyuan9642 | 0:6cb2eaf8b133 | 517 | const LocalEquatorialCoordinates& offset, double cone) |
caoyuan9642 | 0:6cb2eaf8b133 | 518 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 519 | static Transformation t; |
caoyuan9642 | 0:6cb2eaf8b133 | 520 | CelestialMath::getMisalignedPolarAxisTransformation(t, pa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 521 | for (int i = 0; i < N; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 522 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 523 | stars[i] = CelestialMath::applyConeError( |
caoyuan9642 | 0:6cb2eaf8b133 | 524 | CelestialMath::applyMisalignment(t, star_ref[i]), cone) |
caoyuan9642 | 0:6cb2eaf8b133 | 525 | + offset; |
caoyuan9642 | 0:6cb2eaf8b133 | 526 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 527 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 528 | |
caoyuan9642 | 0:6cb2eaf8b133 | 529 | static void fill_jacobian(const int N, const int j, |
caoyuan9642 | 0:6cb2eaf8b133 | 530 | LocalEquatorialCoordinates stars0[], |
caoyuan9642 | 0:6cb2eaf8b133 | 531 | LocalEquatorialCoordinates stars1[], const double &dd) |
caoyuan9642 | 0:6cb2eaf8b133 | 532 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 533 | for (int i = 0; i < N; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 534 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 535 | jac[i * 2][j] = (stars1[i].dec - stars0[i].dec) / dd; |
caoyuan9642 | 0:6cb2eaf8b133 | 536 | jac[i * 2 + 1][j] = (stars1[i].ha - stars0[i].ha) / dd; |
caoyuan9642 | 0:6cb2eaf8b133 | 537 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 538 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 539 | |
caoyuan9642 | 0:6cb2eaf8b133 | 540 | static double det33(int a1, int a2, int a3, int b1, int b2, int b3) |
caoyuan9642 | 0:6cb2eaf8b133 | 541 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 542 | return jacjac[a1][b1] * jacjac[a2][b2] * jacjac[a3][b3] |
caoyuan9642 | 0:6cb2eaf8b133 | 543 | + jacjac[a1][b2] * jacjac[a2][b3] * jacjac[a3][b1] |
caoyuan9642 | 0:6cb2eaf8b133 | 544 | + jacjac[a1][b3] * jacjac[a2][b1] * jacjac[a3][b2] |
caoyuan9642 | 0:6cb2eaf8b133 | 545 | - jacjac[a1][b1] * jacjac[a2][b3] * jacjac[a3][b2] |
caoyuan9642 | 0:6cb2eaf8b133 | 546 | - jacjac[a1][b2] * jacjac[a2][b1] * jacjac[a3][b3] |
caoyuan9642 | 0:6cb2eaf8b133 | 547 | - jacjac[a1][b3] * jacjac[a2][b2] * jacjac[a3][b1]; |
caoyuan9642 | 0:6cb2eaf8b133 | 548 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 549 | |
caoyuan9642 | 0:6cb2eaf8b133 | 550 | static double det44(int a1, int a2, int a3, int a4, int b1, int b2, int b3, |
caoyuan9642 | 0:6cb2eaf8b133 | 551 | int b4) |
caoyuan9642 | 0:6cb2eaf8b133 | 552 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 553 | return jacjac[a1][b1] * det33(a2, a3, a4, b2, b3, b4) |
caoyuan9642 | 0:6cb2eaf8b133 | 554 | - jacjac[a1][b2] * det33(a2, a3, a4, b1, b3, b4) |
caoyuan9642 | 0:6cb2eaf8b133 | 555 | + jacjac[a1][b3] * det33(a2, a3, a4, b1, b2, b4) |
caoyuan9642 | 0:6cb2eaf8b133 | 556 | - jacjac[a1][b4] * det33(a2, a3, a4, b1, b2, b3); |
caoyuan9642 | 0:6cb2eaf8b133 | 557 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 558 | |
caoyuan9642 | 0:6cb2eaf8b133 | 559 | static void invert() |
caoyuan9642 | 0:6cb2eaf8b133 | 560 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 561 | invj[0][0] = det44(1, 2, 3, 4, 1, 2, 3, 4); |
caoyuan9642 | 0:6cb2eaf8b133 | 562 | invj[1][0] = -det44(1, 2, 3, 4, 0, 2, 3, 4); |
caoyuan9642 | 0:6cb2eaf8b133 | 563 | invj[2][0] = det44(1, 2, 3, 4, 0, 1, 3, 4); |
caoyuan9642 | 0:6cb2eaf8b133 | 564 | invj[3][0] = -det44(1, 2, 3, 4, 0, 1, 2, 4); |
caoyuan9642 | 0:6cb2eaf8b133 | 565 | invj[4][0] = det44(1, 2, 3, 4, 0, 1, 2, 3); |
caoyuan9642 | 0:6cb2eaf8b133 | 566 | |
caoyuan9642 | 0:6cb2eaf8b133 | 567 | double det55 = invj[0][0] * jacjac[0][0] + invj[1][0] * jacjac[0][1] |
caoyuan9642 | 0:6cb2eaf8b133 | 568 | + invj[2][0] * jacjac[0][2] + invj[3][0] * jacjac[0][3] |
caoyuan9642 | 0:6cb2eaf8b133 | 569 | + invj[4][0] * jacjac[0][4]; |
caoyuan9642 | 0:6cb2eaf8b133 | 570 | double idet55 = 1.0 / det55; |
caoyuan9642 | 0:6cb2eaf8b133 | 571 | |
caoyuan9642 | 0:6cb2eaf8b133 | 572 | invj[0][0] *= idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 573 | invj[1][0] *= idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 574 | invj[2][0] *= idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 575 | invj[3][0] *= idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 576 | invj[4][0] *= idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 577 | |
caoyuan9642 | 0:6cb2eaf8b133 | 578 | invj[0][1] = -det44(0, 2, 3, 4, 1, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 579 | invj[1][1] = det44(0, 2, 3, 4, 0, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 580 | invj[2][1] = -det44(0, 2, 3, 4, 0, 1, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 581 | invj[3][1] = det44(0, 2, 3, 4, 0, 1, 2, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 582 | invj[4][1] = -det44(0, 2, 3, 4, 0, 1, 2, 3) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 583 | |
caoyuan9642 | 0:6cb2eaf8b133 | 584 | invj[0][2] = det44(0, 1, 3, 4, 1, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 585 | invj[1][2] = -det44(0, 1, 3, 4, 0, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 586 | invj[2][2] = det44(0, 1, 3, 4, 0, 1, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 587 | invj[3][2] = -det44(0, 1, 3, 4, 0, 1, 2, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 588 | invj[4][2] = det44(0, 1, 3, 4, 0, 1, 2, 3) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 589 | |
caoyuan9642 | 0:6cb2eaf8b133 | 590 | invj[0][3] = -det44(0, 1, 2, 4, 1, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 591 | invj[1][3] = det44(0, 1, 2, 4, 0, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 592 | invj[2][3] = -det44(0, 1, 2, 4, 0, 1, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 593 | invj[3][3] = det44(0, 1, 2, 4, 0, 1, 2, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 594 | invj[4][3] = -det44(0, 1, 2, 4, 0, 1, 2, 3) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 595 | |
caoyuan9642 | 0:6cb2eaf8b133 | 596 | invj[0][4] = det44(0, 1, 2, 3, 1, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 597 | invj[1][4] = -det44(0, 1, 2, 3, 0, 2, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 598 | invj[2][4] = det44(0, 1, 2, 3, 0, 1, 3, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 599 | invj[3][4] = -det44(0, 1, 2, 3, 0, 1, 2, 4) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 600 | invj[4][4] = det44(0, 1, 2, 3, 0, 1, 2, 3) * idet55; |
caoyuan9642 | 0:6cb2eaf8b133 | 601 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 602 | |
caoyuan9642 | 0:6cb2eaf8b133 | 603 | static inline double sqr(double x) |
caoyuan9642 | 0:6cb2eaf8b133 | 604 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 605 | return x * x; |
caoyuan9642 | 0:6cb2eaf8b133 | 606 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 607 | |
caoyuan9642 | 0:6cb2eaf8b133 | 608 | void CelestialMath::alignNStars(const int N, |
caoyuan9642 | 0:6cb2eaf8b133 | 609 | const LocalEquatorialCoordinates star_ref[], |
caoyuan9642 | 0:6cb2eaf8b133 | 610 | const LocalEquatorialCoordinates star_meas[], |
caoyuan9642 | 0:6cb2eaf8b133 | 611 | const LocationCoordinates& loc, AzimuthalCoordinates& pa, |
caoyuan9642 | 0:6cb2eaf8b133 | 612 | LocalEquatorialCoordinates& offset, double& cone) |
caoyuan9642 | 0:6cb2eaf8b133 | 613 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 614 | if (N == 2) |
caoyuan9642 | 0:6cb2eaf8b133 | 615 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 616 | alignTwoStars(star_ref, star_meas, loc, pa, offset); |
caoyuan9642 | 0:6cb2eaf8b133 | 617 | cone = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 618 | return; |
caoyuan9642 | 0:6cb2eaf8b133 | 619 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 620 | if (N <= 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 621 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 622 | return; |
caoyuan9642 | 0:6cb2eaf8b133 | 623 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 624 | |
caoyuan9642 | 0:6cb2eaf8b133 | 625 | // Assuming the cone error is not huge, we should be fairly close to the local minimum |
caoyuan9642 | 0:6cb2eaf8b133 | 626 | int i = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 627 | double residue = 1e10; |
caoyuan9642 | 0:6cb2eaf8b133 | 628 | LocalEquatorialCoordinates stars0[N], stars1[N]; |
caoyuan9642 | 0:6cb2eaf8b133 | 629 | double dp[5]; |
caoyuan9642 | 0:6cb2eaf8b133 | 630 | double f[20]; |
caoyuan9642 | 0:6cb2eaf8b133 | 631 | |
caoyuan9642 | 0:6cb2eaf8b133 | 632 | while (i++ < MAX_ITERATION_OPTIMIZATION && residue > tol) |
caoyuan9642 | 0:6cb2eaf8b133 | 633 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 634 | // Calulate Jacobian |
caoyuan9642 | 0:6cb2eaf8b133 | 635 | get_corrected_stars(N, stars0, star_ref, loc, pa, offset, cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 636 | /*Vary pa.alt*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 637 | get_corrected_stars(N, stars1, star_ref, loc, |
caoyuan9642 | 0:6cb2eaf8b133 | 638 | AzimuthalCoordinates(pa.alt + delta, pa.azi), offset, cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 639 | fill_jacobian(N, 0, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 640 | /*Vary pa.azi*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 641 | get_corrected_stars(N, stars1, star_ref, loc, |
caoyuan9642 | 0:6cb2eaf8b133 | 642 | AzimuthalCoordinates(pa.alt, pa.azi + delta), offset, cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 643 | fill_jacobian(N, 1, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 644 | /*Vary offset.dec*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 645 | get_corrected_stars(N, stars1, star_ref, loc, pa, |
caoyuan9642 | 0:6cb2eaf8b133 | 646 | LocalEquatorialCoordinates(offset.dec + delta, offset.ha), |
caoyuan9642 | 0:6cb2eaf8b133 | 647 | cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 648 | fill_jacobian(N, 2, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 649 | /*Vary offset.ha*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 650 | get_corrected_stars(N, stars1, star_ref, loc, pa, |
caoyuan9642 | 0:6cb2eaf8b133 | 651 | LocalEquatorialCoordinates(offset.dec, offset.ha + delta), |
caoyuan9642 | 0:6cb2eaf8b133 | 652 | cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 653 | fill_jacobian(N, 3, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 654 | /*Vary cone*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 655 | get_corrected_stars(N, stars1, star_ref, loc, pa, offset, cone + delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 656 | fill_jacobian(N, 4, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 657 | |
caoyuan9642 | 0:6cb2eaf8b133 | 658 | // The Jacobian is now filled. It is 2*N rows and 5 columns |
caoyuan9642 | 0:6cb2eaf8b133 | 659 | // Gauss-Newton method: x_n - x_(n-1) = - (J'J)^-1J' * f_(n-1) |
caoyuan9642 | 0:6cb2eaf8b133 | 660 | // 1. Matrix multiplication |
caoyuan9642 | 0:6cb2eaf8b133 | 661 | int p, q, r; |
caoyuan9642 | 0:6cb2eaf8b133 | 662 | |
caoyuan9642 | 0:6cb2eaf8b133 | 663 | for (p = 0; p < 5; p++) |
caoyuan9642 | 0:6cb2eaf8b133 | 664 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 665 | for (q = 0; q < 5; q++) |
caoyuan9642 | 0:6cb2eaf8b133 | 666 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 667 | double s = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 668 | for (r = 0; r < 2 * N; r++) |
caoyuan9642 | 0:6cb2eaf8b133 | 669 | s += jac[r][p] * jac[r][q]; |
caoyuan9642 | 0:6cb2eaf8b133 | 670 | jacjac[p][q] = s; |
caoyuan9642 | 0:6cb2eaf8b133 | 671 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 672 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 673 | |
caoyuan9642 | 0:6cb2eaf8b133 | 674 | // 2. Matrix inversion |
caoyuan9642 | 0:6cb2eaf8b133 | 675 | invert(); |
caoyuan9642 | 0:6cb2eaf8b133 | 676 | |
caoyuan9642 | 0:6cb2eaf8b133 | 677 | // 3. Calculate f_(n-1) |
caoyuan9642 | 0:6cb2eaf8b133 | 678 | double newresidue = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 679 | for (p = 0; p < N; p++) |
caoyuan9642 | 0:6cb2eaf8b133 | 680 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 681 | f[2 * p] = stars0[p].dec - star_meas[p].dec; |
caoyuan9642 | 0:6cb2eaf8b133 | 682 | f[2 * p + 1] = stars0[p].ha - star_meas[p].ha; |
caoyuan9642 | 0:6cb2eaf8b133 | 683 | newresidue += sqr(f[2 * p]) + sqr(f[2 * p + 1]); |
caoyuan9642 | 0:6cb2eaf8b133 | 684 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 685 | newresidue = sqrt(newresidue); |
caoyuan9642 | 0:6cb2eaf8b133 | 686 | // 4. Matrix multiplication |
caoyuan9642 | 0:6cb2eaf8b133 | 687 | for (p = 0; p < 5; p++) |
caoyuan9642 | 0:6cb2eaf8b133 | 688 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 689 | double s = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 690 | for (q = 0; q < 5; q++) |
caoyuan9642 | 0:6cb2eaf8b133 | 691 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 692 | for (r = 0; r < 2 * N; r++) |
caoyuan9642 | 0:6cb2eaf8b133 | 693 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 694 | s += invj[p][q] * jac[r][q] * f[r]; |
caoyuan9642 | 0:6cb2eaf8b133 | 695 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 696 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 697 | dp[p] = -s; |
caoyuan9642 | 0:6cb2eaf8b133 | 698 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 699 | // 5. Apply the correction |
caoyuan9642 | 0:6cb2eaf8b133 | 700 | pa.alt += dp[0]; |
caoyuan9642 | 0:6cb2eaf8b133 | 701 | pa.azi += dp[1]; |
caoyuan9642 | 0:6cb2eaf8b133 | 702 | offset.dec += dp[2]; |
caoyuan9642 | 0:6cb2eaf8b133 | 703 | offset.ha += dp[3]; |
caoyuan9642 | 0:6cb2eaf8b133 | 704 | cone += dp[4]; |
caoyuan9642 | 0:6cb2eaf8b133 | 705 | |
caoyuan9642 | 0:6cb2eaf8b133 | 706 | if (newresidue >= residue - tol) |
caoyuan9642 | 0:6cb2eaf8b133 | 707 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 708 | debug_if(CM_DEBUG, "Converged.\n"); |
caoyuan9642 | 0:6cb2eaf8b133 | 709 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 710 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 711 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 712 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 713 | residue = newresidue; |
caoyuan9642 | 0:6cb2eaf8b133 | 714 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 715 | debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\t%f\tr=%f\n", i, |
caoyuan9642 | 0:6cb2eaf8b133 | 716 | pa.alt, pa.azi, offset.dec, offset.ha, cone, residue); |
caoyuan9642 | 0:6cb2eaf8b133 | 717 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 718 | |
caoyuan9642 | 0:6cb2eaf8b133 | 719 | debug_if(CM_DEBUG, "Final result: %f\t%f\t%f\t%f\t%f\tr=%f\n", pa.alt, |
caoyuan9642 | 0:6cb2eaf8b133 | 720 | pa.azi, offset.dec, offset.ha, cone, residue); |
caoyuan9642 | 0:6cb2eaf8b133 | 721 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 722 | |
caoyuan9642 | 0:6cb2eaf8b133 | 723 | static void get_corrected_stars(const int N, MountCoordinates stars[], |
caoyuan9642 | 0:6cb2eaf8b133 | 724 | const LocalEquatorialCoordinates star_ref[], |
caoyuan9642 | 0:6cb2eaf8b133 | 725 | const MountCoordinates star_meas[], const LocationCoordinates& loc, |
caoyuan9642 | 0:6cb2eaf8b133 | 726 | const AzimuthalCoordinates& pa, const IndexOffset& offset, double cone) |
caoyuan9642 | 0:6cb2eaf8b133 | 727 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 728 | static Transformation t; |
caoyuan9642 | 0:6cb2eaf8b133 | 729 | CelestialMath::getMisalignedPolarAxisTransformation(t, pa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 730 | for (int i = 0; i < N; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 731 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 732 | // Misalign, apply cone error, and transform to mount coordinates using the same pier side as in the measured stars |
caoyuan9642 | 0:6cb2eaf8b133 | 733 | stars[i] = CelestialMath::localEquatorialToMount( |
caoyuan9642 | 0:6cb2eaf8b133 | 734 | CelestialMath::applyConeError( |
caoyuan9642 | 0:6cb2eaf8b133 | 735 | CelestialMath::applyMisalignment(t, star_ref[i]), cone), |
caoyuan9642 | 0:6cb2eaf8b133 | 736 | star_meas[i].side) + offset; |
caoyuan9642 | 0:6cb2eaf8b133 | 737 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 738 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 739 | |
caoyuan9642 | 0:6cb2eaf8b133 | 740 | static void fill_jacobian(const int N, const int j, MountCoordinates stars0[], |
caoyuan9642 | 0:6cb2eaf8b133 | 741 | MountCoordinates stars1[], const double &dd) |
caoyuan9642 | 0:6cb2eaf8b133 | 742 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 743 | for (int i = 0; i < N; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 744 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 745 | jac[i * 2][j] = (stars1[i].dec_delta - stars0[i].dec_delta) / dd; |
caoyuan9642 | 0:6cb2eaf8b133 | 746 | jac[i * 2 + 1][j] = (stars1[i].ra_delta - stars0[i].ra_delta) / dd; |
caoyuan9642 | 0:6cb2eaf8b133 | 747 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 748 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 749 | |
caoyuan9642 | 0:6cb2eaf8b133 | 750 | void CelestialMath::alignNStars(const int N, |
caoyuan9642 | 0:6cb2eaf8b133 | 751 | const LocalEquatorialCoordinates star_ref[], |
caoyuan9642 | 0:6cb2eaf8b133 | 752 | const MountCoordinates star_meas[], const LocationCoordinates& loc, |
caoyuan9642 | 0:6cb2eaf8b133 | 753 | AzimuthalCoordinates& pa, IndexOffset& offset, double& cone, |
caoyuan9642 | 0:6cb2eaf8b133 | 754 | bool &diverge) |
caoyuan9642 | 0:6cb2eaf8b133 | 755 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 756 | if (N == 2) |
caoyuan9642 | 0:6cb2eaf8b133 | 757 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 758 | alignTwoStars(star_ref, star_meas, loc, pa, offset, diverge); |
caoyuan9642 | 0:6cb2eaf8b133 | 759 | cone = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 760 | return; |
caoyuan9642 | 0:6cb2eaf8b133 | 761 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 762 | if (N <= 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 763 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 764 | return; |
caoyuan9642 | 0:6cb2eaf8b133 | 765 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 766 | |
caoyuan9642 | 0:6cb2eaf8b133 | 767 | // Assuming the cone error is not huge, we should be fairly close to the local minimum |
caoyuan9642 | 0:6cb2eaf8b133 | 768 | int i = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 769 | double residue = 1e10; |
caoyuan9642 | 0:6cb2eaf8b133 | 770 | MountCoordinates stars0[N], stars1[N]; |
caoyuan9642 | 0:6cb2eaf8b133 | 771 | double dp[5]; |
caoyuan9642 | 0:6cb2eaf8b133 | 772 | double f[20]; |
caoyuan9642 | 0:6cb2eaf8b133 | 773 | |
caoyuan9642 | 0:6cb2eaf8b133 | 774 | diverge = true; |
caoyuan9642 | 0:6cb2eaf8b133 | 775 | |
caoyuan9642 | 0:6cb2eaf8b133 | 776 | while (i++ < MAX_ITERATION_OPTIMIZATION && residue > tol) |
caoyuan9642 | 0:6cb2eaf8b133 | 777 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 778 | // Calulate Jacobian |
caoyuan9642 | 0:6cb2eaf8b133 | 779 | get_corrected_stars(N, stars0, star_ref, star_meas, loc, pa, offset, |
caoyuan9642 | 0:6cb2eaf8b133 | 780 | cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 781 | /*Vary pa.alt*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 782 | get_corrected_stars(N, stars1, star_ref, star_meas, loc, |
caoyuan9642 | 0:6cb2eaf8b133 | 783 | AzimuthalCoordinates(pa.alt + delta, pa.azi), offset, cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 784 | fill_jacobian(N, 0, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 785 | /*Vary pa.azi*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 786 | get_corrected_stars(N, stars1, star_ref, star_meas, loc, |
caoyuan9642 | 0:6cb2eaf8b133 | 787 | AzimuthalCoordinates(pa.alt, pa.azi + delta), offset, cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 788 | fill_jacobian(N, 1, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 789 | /*Vary offset.dec*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 790 | get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa, |
caoyuan9642 | 0:6cb2eaf8b133 | 791 | IndexOffset(offset.dec_off + delta, offset.ra_off), cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 792 | fill_jacobian(N, 2, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 793 | /*Vary offset.ha*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 794 | get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa, |
caoyuan9642 | 0:6cb2eaf8b133 | 795 | IndexOffset(offset.dec_off, offset.ra_off + delta), cone); |
caoyuan9642 | 0:6cb2eaf8b133 | 796 | fill_jacobian(N, 3, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 797 | /*Vary cone*/ |
caoyuan9642 | 0:6cb2eaf8b133 | 798 | get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa, offset, |
caoyuan9642 | 0:6cb2eaf8b133 | 799 | cone + delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 800 | fill_jacobian(N, 4, stars0, stars1, delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 801 | |
caoyuan9642 | 0:6cb2eaf8b133 | 802 | // The Jacobian is now filled. It is 2*N rows and 5 columns |
caoyuan9642 | 0:6cb2eaf8b133 | 803 | // Gauss-Newton method: x_n - x_(n-1) = - (J'J)^-1J' * f_(n-1) |
caoyuan9642 | 0:6cb2eaf8b133 | 804 | // 1. Matrix multiplication |
caoyuan9642 | 0:6cb2eaf8b133 | 805 | int p, q, r; |
caoyuan9642 | 0:6cb2eaf8b133 | 806 | |
caoyuan9642 | 0:6cb2eaf8b133 | 807 | for (p = 0; p < 5; p++) |
caoyuan9642 | 0:6cb2eaf8b133 | 808 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 809 | for (q = 0; q < 5; q++) |
caoyuan9642 | 0:6cb2eaf8b133 | 810 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 811 | double s = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 812 | for (r = 0; r < 2 * N; r++) |
caoyuan9642 | 0:6cb2eaf8b133 | 813 | s += jac[r][p] * jac[r][q]; |
caoyuan9642 | 0:6cb2eaf8b133 | 814 | jacjac[p][q] = s; |
caoyuan9642 | 0:6cb2eaf8b133 | 815 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 816 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 817 | |
caoyuan9642 | 0:6cb2eaf8b133 | 818 | // 2. Matrix inversion |
caoyuan9642 | 0:6cb2eaf8b133 | 819 | invert(); |
caoyuan9642 | 0:6cb2eaf8b133 | 820 | |
caoyuan9642 | 0:6cb2eaf8b133 | 821 | // 3. Calculate f_(n-1) |
caoyuan9642 | 0:6cb2eaf8b133 | 822 | double newresidue = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 823 | for (p = 0; p < N; p++) |
caoyuan9642 | 0:6cb2eaf8b133 | 824 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 825 | f[2 * p] = stars0[p].dec_delta - star_meas[p].dec_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 826 | f[2 * p + 1] = stars0[p].ra_delta - star_meas[p].ra_delta; |
caoyuan9642 | 0:6cb2eaf8b133 | 827 | newresidue += sqr(f[2 * p]) + sqr(f[2 * p + 1]); |
caoyuan9642 | 0:6cb2eaf8b133 | 828 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 829 | newresidue = sqrt(newresidue); |
caoyuan9642 | 0:6cb2eaf8b133 | 830 | // 4. Matrix multiplication |
caoyuan9642 | 0:6cb2eaf8b133 | 831 | for (p = 0; p < 5; p++) |
caoyuan9642 | 0:6cb2eaf8b133 | 832 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 833 | double s = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 834 | for (q = 0; q < 5; q++) |
caoyuan9642 | 0:6cb2eaf8b133 | 835 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 836 | for (r = 0; r < 2 * N; r++) |
caoyuan9642 | 0:6cb2eaf8b133 | 837 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 838 | s += invj[p][q] * jac[r][q] * f[r]; |
caoyuan9642 | 0:6cb2eaf8b133 | 839 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 840 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 841 | dp[p] = -s; |
caoyuan9642 | 0:6cb2eaf8b133 | 842 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 843 | // 5. Apply the correction |
caoyuan9642 | 0:6cb2eaf8b133 | 844 | pa.alt += dp[0]; |
caoyuan9642 | 0:6cb2eaf8b133 | 845 | pa.azi += dp[1]; |
caoyuan9642 | 0:6cb2eaf8b133 | 846 | offset.dec_off += dp[2]; |
caoyuan9642 | 0:6cb2eaf8b133 | 847 | offset.ra_off += dp[3]; |
caoyuan9642 | 0:6cb2eaf8b133 | 848 | cone += dp[4]; |
caoyuan9642 | 0:6cb2eaf8b133 | 849 | |
caoyuan9642 | 0:6cb2eaf8b133 | 850 | if (newresidue >= residue - tol) |
caoyuan9642 | 0:6cb2eaf8b133 | 851 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 852 | debug_if(CM_DEBUG, "Converged.\n"); |
caoyuan9642 | 0:6cb2eaf8b133 | 853 | diverge = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 854 | break; |
caoyuan9642 | 0:6cb2eaf8b133 | 855 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 856 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 857 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 858 | residue = newresidue; |
caoyuan9642 | 0:6cb2eaf8b133 | 859 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 860 | debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\t%f\tr=%f\n", i, |
caoyuan9642 | 0:6cb2eaf8b133 | 861 | pa.alt, pa.azi, offset.dec_off, offset.ra_off, cone, residue); |
caoyuan9642 | 0:6cb2eaf8b133 | 862 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 863 | |
caoyuan9642 | 0:6cb2eaf8b133 | 864 | if (diverge) |
caoyuan9642 | 0:6cb2eaf8b133 | 865 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 866 | debug_if(CM_DEBUG, "Diverged.\n"); |
caoyuan9642 | 0:6cb2eaf8b133 | 867 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 868 | |
caoyuan9642 | 0:6cb2eaf8b133 | 869 | debug_if(CM_DEBUG, "Final result: %f\t%f\t%f\t%f\t%f\tr=%f\n", pa.alt, |
caoyuan9642 | 0:6cb2eaf8b133 | 870 | pa.azi, offset.dec_off, offset.ra_off, cone, residue); |
caoyuan9642 | 0:6cb2eaf8b133 | 871 | |
caoyuan9642 | 0:6cb2eaf8b133 | 872 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 873 | |
caoyuan9642 | 0:6cb2eaf8b133 | 874 | EqCalibration CelestialMath::align(const int N, const AlignmentStar stars[], |
caoyuan9642 | 0:6cb2eaf8b133 | 875 | const LocationCoordinates &loc, bool &diverge) |
caoyuan9642 | 0:6cb2eaf8b133 | 876 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 877 | EqCalibration calib; |
caoyuan9642 | 0:6cb2eaf8b133 | 878 | calib.pa.alt = loc.lat; |
caoyuan9642 | 0:6cb2eaf8b133 | 879 | if (N == 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 880 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 881 | calib.offset = alignOneStarForOffset(stars[0].star_ref_local(loc), |
caoyuan9642 | 0:6cb2eaf8b133 | 882 | stars[0].star_meas); |
caoyuan9642 | 0:6cb2eaf8b133 | 883 | diverge = false; |
caoyuan9642 | 0:6cb2eaf8b133 | 884 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 885 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 886 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 887 | LocalEquatorialCoordinates star_ref[N]; |
caoyuan9642 | 0:6cb2eaf8b133 | 888 | MountCoordinates star_meas[N]; |
caoyuan9642 | 0:6cb2eaf8b133 | 889 | for (int i = 0; i < N; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 890 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 891 | star_ref[i] = stars[i].star_ref_local(loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 892 | star_meas[i] = stars[i].star_meas; |
caoyuan9642 | 0:6cb2eaf8b133 | 893 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 894 | if (N == 2) |
caoyuan9642 | 0:6cb2eaf8b133 | 895 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 896 | alignTwoStars(star_ref, star_meas, loc, calib.pa, calib.offset, |
caoyuan9642 | 0:6cb2eaf8b133 | 897 | diverge); |
caoyuan9642 | 0:6cb2eaf8b133 | 898 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 899 | else |
caoyuan9642 | 0:6cb2eaf8b133 | 900 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 901 | alignNStars(N, star_ref, star_meas, loc, calib.pa, calib.offset, |
caoyuan9642 | 0:6cb2eaf8b133 | 902 | calib.cone, diverge); |
caoyuan9642 | 0:6cb2eaf8b133 | 903 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 904 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 905 | calib.error = CelestialMath::alignmentError(N, stars, calib, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 906 | |
caoyuan9642 | 0:6cb2eaf8b133 | 907 | return calib; |
caoyuan9642 | 0:6cb2eaf8b133 | 908 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 909 | |
caoyuan9642 | 0:6cb2eaf8b133 | 910 | double CelestialMath::parseHMSAngle(char* hms) |
caoyuan9642 | 0:6cb2eaf8b133 | 911 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 912 | char *h = strchr(hms, 'h'); |
caoyuan9642 | 0:6cb2eaf8b133 | 913 | char *m = strchr(hms, 'm'); |
caoyuan9642 | 0:6cb2eaf8b133 | 914 | char *s = strchr(hms, 's'); |
caoyuan9642 | 0:6cb2eaf8b133 | 915 | if (h == NULL || m == NULL || s == NULL || !(h < m && m < s)) |
caoyuan9642 | 0:6cb2eaf8b133 | 916 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 917 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 918 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 919 | |
caoyuan9642 | 0:6cb2eaf8b133 | 920 | *h = '\0'; |
caoyuan9642 | 0:6cb2eaf8b133 | 921 | *m = '\0'; |
caoyuan9642 | 0:6cb2eaf8b133 | 922 | *s = '\0'; |
caoyuan9642 | 0:6cb2eaf8b133 | 923 | |
caoyuan9642 | 0:6cb2eaf8b133 | 924 | char *tp; |
caoyuan9642 | 0:6cb2eaf8b133 | 925 | int hour = strtol(hms, &tp, 10); |
caoyuan9642 | 0:6cb2eaf8b133 | 926 | if (tp == hms) |
caoyuan9642 | 0:6cb2eaf8b133 | 927 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 928 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 929 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 930 | int minute = strtol(h + 1, &tp, 10); |
caoyuan9642 | 0:6cb2eaf8b133 | 931 | if (tp == h + 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 932 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 933 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 934 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 935 | double second = strtod(m + 1, &tp); |
caoyuan9642 | 0:6cb2eaf8b133 | 936 | if (tp == m + 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 937 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 938 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 939 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 940 | |
caoyuan9642 | 0:6cb2eaf8b133 | 941 | if (!(hour >= 0 && hour <= 23 && minute >= 0 && minute <= 59 && second >= 0 |
caoyuan9642 | 0:6cb2eaf8b133 | 942 | && second <= 60)) |
caoyuan9642 | 0:6cb2eaf8b133 | 943 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 944 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 945 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 946 | |
caoyuan9642 | 0:6cb2eaf8b133 | 947 | return remainder((hour + minute / 60.0 + second / 3600.0) * 15, 360); |
caoyuan9642 | 0:6cb2eaf8b133 | 948 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 949 | |
caoyuan9642 | 0:6cb2eaf8b133 | 950 | double CelestialMath::parseDMSAngle(char* dms) |
caoyuan9642 | 0:6cb2eaf8b133 | 951 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 952 | char *d = strchr(dms, 'd'); |
caoyuan9642 | 0:6cb2eaf8b133 | 953 | char *m = strchr(dms, 'm'); |
caoyuan9642 | 0:6cb2eaf8b133 | 954 | char *s = strchr(dms, 's'); |
caoyuan9642 | 0:6cb2eaf8b133 | 955 | if (d == NULL || m == NULL || s == NULL || !(d < m && m < s)) |
caoyuan9642 | 0:6cb2eaf8b133 | 956 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 957 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 958 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 959 | |
caoyuan9642 | 0:6cb2eaf8b133 | 960 | *d = '\0'; |
caoyuan9642 | 0:6cb2eaf8b133 | 961 | *m = '\0'; |
caoyuan9642 | 0:6cb2eaf8b133 | 962 | *s = '\0'; |
caoyuan9642 | 0:6cb2eaf8b133 | 963 | |
caoyuan9642 | 0:6cb2eaf8b133 | 964 | char *tp; |
caoyuan9642 | 0:6cb2eaf8b133 | 965 | int degree = strtol(dms, &tp, 10); |
caoyuan9642 | 0:6cb2eaf8b133 | 966 | if (tp == dms) |
caoyuan9642 | 0:6cb2eaf8b133 | 967 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 968 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 969 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 970 | int arcminute = strtol(d + 1, &tp, 10); |
caoyuan9642 | 0:6cb2eaf8b133 | 971 | if (tp == d + 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 972 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 973 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 974 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 975 | double arcsecond = strtod(m + 1, &tp); |
caoyuan9642 | 0:6cb2eaf8b133 | 976 | if (tp == m + 1) |
caoyuan9642 | 0:6cb2eaf8b133 | 977 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 978 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 979 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 980 | |
caoyuan9642 | 0:6cb2eaf8b133 | 981 | if (!(degree >= -180.0 && degree <= 180.0 && arcminute >= 0 |
caoyuan9642 | 0:6cb2eaf8b133 | 982 | && arcminute <= 59 && arcsecond >= 0 && arcsecond <= 60)) |
caoyuan9642 | 0:6cb2eaf8b133 | 983 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 984 | return NAN; |
caoyuan9642 | 0:6cb2eaf8b133 | 985 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 986 | |
caoyuan9642 | 0:6cb2eaf8b133 | 987 | return remainder((degree + arcminute / 60.0 + arcsecond / 3600.0), 360); |
caoyuan9642 | 0:6cb2eaf8b133 | 988 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 989 | |
caoyuan9642 | 0:6cb2eaf8b133 | 990 | double CelestialMath::kingRate(EquatorialCoordinates eq, |
caoyuan9642 | 0:6cb2eaf8b133 | 991 | LocationCoordinates loc, time_t time) |
caoyuan9642 | 0:6cb2eaf8b133 | 992 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 993 | LocalEquatorialCoordinates leq = CelestialMath::equatorialToLocalEquatorial( |
caoyuan9642 | 0:6cb2eaf8b133 | 994 | eq, time, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 995 | // AzimuthalCoordinates ac = CelestialMath::localEquatorialToAzimuthal(leq, |
caoyuan9642 | 0:6cb2eaf8b133 | 996 | // loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 997 | double cosLat = cos(loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 998 | double sinLat = sin(loc.lat * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 999 | double cotLat = cosLat / sinLat; |
caoyuan9642 | 0:6cb2eaf8b133 | 1000 | double cosDec = cos(eq.dec * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 1001 | double sinDec = sin(eq.dec * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 1002 | double tanDec = sinDec / cosDec; |
caoyuan9642 | 0:6cb2eaf8b133 | 1003 | double cosHA = cos(leq.ha * DEGREE); |
caoyuan9642 | 0:6cb2eaf8b133 | 1004 | double kingMpD = (1436.46 |
caoyuan9642 | 0:6cb2eaf8b133 | 1005 | + 0.4 |
caoyuan9642 | 0:6cb2eaf8b133 | 1006 | * (cosLat / cosDec |
caoyuan9642 | 0:6cb2eaf8b133 | 1007 | * (cosLat * cosDec + sinLat * sinDec * cosHA) |
caoyuan9642 | 0:6cb2eaf8b133 | 1008 | / pow(sinLat * sinDec + cosLat * cosDec * cosHA, |
caoyuan9642 | 0:6cb2eaf8b133 | 1009 | 2.0) - cotLat * tanDec * cosHA)); |
caoyuan9642 | 0:6cb2eaf8b133 | 1010 | return 6.0 / kingMpD; |
caoyuan9642 | 0:6cb2eaf8b133 | 1011 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 1012 | |
caoyuan9642 | 0:6cb2eaf8b133 | 1013 | double CelestialMath::alignmentError(const int N, const AlignmentStar stars[], |
caoyuan9642 | 0:6cb2eaf8b133 | 1014 | const EqCalibration &calib, const LocationCoordinates& loc) |
caoyuan9642 | 0:6cb2eaf8b133 | 1015 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 1016 | static Transformation t; |
caoyuan9642 | 0:6cb2eaf8b133 | 1017 | CelestialMath::getMisalignedPolarAxisTransformation(t, calib.pa, loc); |
caoyuan9642 | 0:6cb2eaf8b133 | 1018 | double r = 0; |
caoyuan9642 | 0:6cb2eaf8b133 | 1019 | for (int i = 0; i < N; i++) |
caoyuan9642 | 0:6cb2eaf8b133 | 1020 | { |
caoyuan9642 | 0:6cb2eaf8b133 | 1021 | // Misalign, apply cone error, and transform to mount coordinates using the same pier side as in the measured stars |
caoyuan9642 | 0:6cb2eaf8b133 | 1022 | MountCoordinates mc = CelestialMath::localEquatorialToMount( |
caoyuan9642 | 0:6cb2eaf8b133 | 1023 | CelestialMath::applyConeError( |
caoyuan9642 | 0:6cb2eaf8b133 | 1024 | CelestialMath::applyMisalignment(t, |
caoyuan9642 | 0:6cb2eaf8b133 | 1025 | CelestialMath::equatorialToLocalEquatorial( |
caoyuan9642 | 0:6cb2eaf8b133 | 1026 | stars[i].star_ref, stars[i].timestamp, |
caoyuan9642 | 0:6cb2eaf8b133 | 1027 | loc)), calib.cone), |
caoyuan9642 | 0:6cb2eaf8b133 | 1028 | stars[i].star_meas.side) + calib.offset; |
caoyuan9642 | 0:6cb2eaf8b133 | 1029 | |
caoyuan9642 | 0:6cb2eaf8b133 | 1030 | r += sqr(mc.ra_delta - stars[i].star_meas.ra_delta) |
caoyuan9642 | 0:6cb2eaf8b133 | 1031 | + sqr(mc.dec_delta - stars[i].star_meas.dec_delta); |
caoyuan9642 | 0:6cb2eaf8b133 | 1032 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 1033 | return sqrt(r); |
caoyuan9642 | 0:6cb2eaf8b133 | 1034 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 1035 |