Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

CelestialMath.cpp@19:fd854309cb4c, 2018-09-24 (annotated)

Committer:
caoyu@caoyuan9642-desktop.MIT.EDU
Date:
Mon Sep 24 19:36:48 2018 -0400
Revision:
19:fd854309cb4c
Parent:
18:3ea58b079adc 
Fix bug in nudging with small speeds mentioned in the last commit

Who changed what in which revision?

UserRevisionLine numberNew 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
caoyu@caoyuan9642-desktop.MIT.EDU 18:3ea58b079adc 86 double CelestialMath::getGreenwichMeanSiderealTime(double 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
caoyu@caoyuan9642-desktop.MIT.EDU 18:3ea58b079adc 98 double CelestialMath::getLocalSiderealTime(double 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(
caoyu@caoyuan9642-desktop.MIT.EDU 18:3ea58b079adc 107 const EquatorialCoordinates &e, double 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(
caoyu@caoyuan9642-desktop.MIT.EDU 18:3ea58b079adc 116 const LocalEquatorialCoordinates &a, double 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,
caoyu@caoyuan9642-desktop.MIT.EDU 18:3ea58b079adc 991 LocationCoordinates loc, double 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