Telescope Control Library

Dependents:   PushToGo-F429

CelestialMath.cpp

Committer:
caoyu@caoyuan9642-desktop.MIT.EDU
Date:
2018-09-24
Revision:
19:fd854309cb4c
Parent:
18:3ea58b079adc

File content as of revision 19:fd854309cb4c:

/*
 * CelestialMath.cpp
 *
 *  Created on: Feb 21, 2018
 *      Author: Yuan
 */

#include "CelestialMath.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "mbed.h"

#define CM_DEBUG 0

static inline double clamp(double x)
{
	return (x > 1) ? 1 : ((x < -1) ? -1 : x);
}

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#define MAX_ITERATION 30
#define MAX_ITERATION_OPTIMIZATION 10

static const double tol = 1e-10;
static const double eps = 1e-13;
static const double delta = 1e-7;

static const double RADIAN = 180.0 / M_PI;
static const double DEGREE = M_PI / 180.0;

CartesianVector CartesianVector::operator*(const Transformation &t)
{
	return CartesianVector(t.a11 * x + t.a21 * y + t.a31 * z,
			t.a12 * x + t.a22 * y + t.a32 * z,
			t.a13 * x + t.a23 * y + t.a33 * z);
}

CartesianVector Transformation::operator*(const CartesianVector &vec)
{ // Left-product of matrix and vector
	return CartesianVector(a11 * vec.x + a12 * vec.y + a13 * vec.z,
			a21 * vec.x + a22 * vec.y + a23 * vec.z,
			a31 * vec.x + a32 * vec.y + a33 * vec.z);
}

AzimuthalCoordinates CelestialMath::localEquatorialToAzimuthal(
		const LocalEquatorialCoordinates &a, const LocationCoordinates &loc)
{
	//              cphi             lambda             lambda0
	double c1 = cos(a.ha * DEGREE), c2 = cos(a.dec * DEGREE), c3 = cos(
			loc.lat * DEGREE);
	double s1 = sin(a.ha * DEGREE), s2 = sin(a.dec * DEGREE), s3 = sin(
			loc.lat * DEGREE);
	double s4 = c1 * c2 * c3 + s2 * s3;
	s4 = clamp(s4);
	double y1 = s1 * c2, x1 = s2 * c3 - c1 * c2 * s3;

	return AzimuthalCoordinates(asin(clamp(s4)) * RADIAN,
			atan2(y1, x1) * RADIAN);
}

LocalEquatorialCoordinates CelestialMath::azimuthalToLocalEquatorial(
		const AzimuthalCoordinates &b, const LocationCoordinates &loc)
{
	//              mu             eps             lambda0
	double c1 = cos(b.azi * DEGREE), c2 = cos(b.alt * DEGREE), c3 = cos(
			loc.lat * DEGREE);
	double s1 = sin(b.azi * DEGREE), s2 = sin(b.alt * DEGREE), s3 = sin(
			loc.lat * DEGREE);
	double s4 = c1 * c2 * c3 + s2 * s3;
	double y1 = s1 * c2, x1 = s2 * c3 - c1 * c2 * s3;

	return LocalEquatorialCoordinates(asin(clamp(s4)) * RADIAN,
			atan2(y1, x1) * RADIAN);
}

LocalEquatorialCoordinates AlignmentStar::star_ref_local(
		const LocationCoordinates &loc) const
{
	return CelestialMath::equatorialToLocalEquatorial(star_ref, timestamp, loc);
}

double CelestialMath::getGreenwichMeanSiderealTime(double timestamp)
{
	double jd = (double) timestamp * 1.1574074074074E-5 + 2440587.5 - 2451545.0; // Julian Date since J2000
//	double jd0 = floor(jd - 0.5) + 0.5; // JD of previous midnight
//	double cent = (jd - 2451545.0) / 36525;
//	double gmst = 6.697374558 + 0.06570982441908 * (jd0 - 2451545.0)
//			+ 1.00273790935 * (jd - jd0) * 24 + 0.000026 * cent * cent;
//	gmst *= 15.0;
	double gmst = 280.46061837 + 360.985647366 * jd; // Greenwich mean sidereal time (angle)
	return remainder(gmst, 360.0);
}

double CelestialMath::getLocalSiderealTime(double timestamp,
		const LocationCoordinates &loc)
{
	double gmst = getGreenwichMeanSiderealTime(timestamp);
	double lst = gmst + loc.lon * 1.00273790935; // Local sidereal time (angle)
	return remainder(lst, 360.0);
}

LocalEquatorialCoordinates CelestialMath::equatorialToLocalEquatorial(
		const EquatorialCoordinates &e, double timestamp,
		const LocationCoordinates &loc)
{
// From phi to cphi
	return LocalEquatorialCoordinates(e.dec,
			remainder(getLocalSiderealTime(timestamp, loc) - e.ra, 360.0));
}

EquatorialCoordinates CelestialMath::localEquatorialToEquatorial(
		const LocalEquatorialCoordinates &a, double timestamp,
		const LocationCoordinates &loc)
{
// From cphi to phi
	return EquatorialCoordinates(a.dec,
			remainder(getLocalSiderealTime(timestamp, loc) - a.ha, 360.0));
}

Transformation &CelestialMath::getMisalignedPolarAxisTransformation(
		Transformation &t, const AzimuthalCoordinates &p,
		const LocationCoordinates &loc)
{
	double c1 = cos(p.azi * DEGREE), c2 = cos(p.alt * DEGREE), c3 = cos(
			loc.lat * DEGREE);
	double s1 = sin(p.azi * DEGREE), s2 = sin(p.alt * DEGREE), s3 = sin(
			loc.lat * DEGREE);
// Matrix to convert from basis vectors in misaligned PA to correct PA
	t.a11 = c1 * s2 * s3 + c2 * c3;
	t.a12 = -s1 * s3;
	t.a13 = -c1 * c2 * s3 + s2 * c3;
	t.a21 = s1 * s2;
	t.a22 = c1;
	t.a23 = -s1 * c2;
	t.a31 = -c1 * s2 * c3 + c2 * s3;
	t.a32 = s1 * c3;
	t.a33 = c1 * c2 * c3 + s2 * s3;
	return t;
}

LocalEquatorialCoordinates CelestialMath::applyMisalignment(
		const Transformation &t, const LocalEquatorialCoordinates& a)
{
	double c1 = cos(a.dec * DEGREE), c2 = cos(a.ha * DEGREE);
	double s1 = sin(a.dec * DEGREE), s2 = sin(a.ha * DEGREE);
	CartesianVector X = CartesianVector(c1 * c2, -c1 * s2, s1) * t;

	return LocalEquatorialCoordinates(asin(clamp(X.z)) * RADIAN,
			atan2(-X.y, X.x) * RADIAN);
}

LocalEquatorialCoordinates CelestialMath::deapplyMisalignment(
		const Transformation &t, const LocalEquatorialCoordinates& a)
{
// the Transformation is ORTHOGONAL, T^-1 = T'
	double c1 = cos(a.dec * DEGREE), c2 = cos(a.ha * DEGREE);
	double s1 = sin(a.dec * DEGREE), s2 = sin(a.ha * DEGREE);
	Transformation tp = t;
	tp.transpose();
	CartesianVector X = CartesianVector(c1 * c2, -c1 * s2, s1) * tp;

	return LocalEquatorialCoordinates(asin(clamp(X.z)) * RADIAN,
			atan2(-X.y, X.x) * RADIAN);
}

LocalEquatorialCoordinates CelestialMath::applyMisalignment(
		const LocalEquatorialCoordinates& a, const AzimuthalCoordinates& mpa,
		const LocationCoordinates& loc)
{
	Transformation tr;
	getMisalignedPolarAxisTransformation(tr, mpa, loc);
	return applyMisalignment(tr, a);
}

LocalEquatorialCoordinates CelestialMath::deapplyMisalignment(
		const LocalEquatorialCoordinates& a, const AzimuthalCoordinates& mpa,
		const LocationCoordinates& loc)
{
	Transformation tr;
	getMisalignedPolarAxisTransformation(tr, mpa, loc);
	return deapplyMisalignment(tr, a);
}

LocalEquatorialCoordinates CelestialMath::applyConeError(
		const LocalEquatorialCoordinates& a, double cone)
{
	return LocalEquatorialCoordinates(
			asin(clamp(sin(a.dec * DEGREE) / cos(cone * DEGREE))) * RADIAN,
			a.ha
					- asin(clamp(tan(a.dec * DEGREE) * tan(cone * DEGREE)))
							* RADIAN);
}

LocalEquatorialCoordinates CelestialMath::deapplyConeError(
		const LocalEquatorialCoordinates& a, double cone)
{
	double lmd = asin(clamp(sin(a.dec * DEGREE) * cos(cone * DEGREE))) * RADIAN;
	if (lmd > 90 - eps || lmd < -90 + eps) // This implies cone=0, so we don't do anything
		return a;
	double phi = a.ha
			+ asin(clamp(tan(cone * DEGREE) * tan(lmd * DEGREE))) * RADIAN;
	return LocalEquatorialCoordinates(lmd, phi);
}

MountCoordinates CelestialMath::localEquatorialToMount(
		const LocalEquatorialCoordinates& a, pierside_t side)
{
	MountCoordinates m;
	double ha = a.ha;
	if (side == PIER_SIDE_WEST
			|| (side == PIER_SIDE_AUTO && (ha = remainder(a.ha, 360.0)) > 0))
	{
		m.side = PIER_SIDE_WEST;
		m.dec_delta = 90.0 - a.dec; // dec_delta > 0
		m.ra_delta = ha - 90.0;
	}
	else
	{
		m.side = PIER_SIDE_EAST;
		m.dec_delta = a.dec - 90; // dec_delta<0
		m.ra_delta = ha + 90.0;
	}
	return m;
}

LocalEquatorialCoordinates CelestialMath::mountToLocalEquatorial(
		const MountCoordinates& m)
{
	LocalEquatorialCoordinates a;
	if (m.side == PIER_SIDE_WEST
			|| (m.side == PIER_SIDE_AUTO && (m.dec_delta > 0)))
	{
		a.ha = m.ra_delta + 90.0;
		a.dec = 90.0 - m.dec_delta;
	}
	else
	{
		a.ha = m.ra_delta - 90;
		a.dec = 90.0 + m.dec_delta;
	}
	return a;
}

AzimuthalCoordinates CelestialMath::alignOneStar(
		const LocalEquatorialCoordinates &star_ref,
		const LocalEquatorialCoordinates &star_meas,
		const LocationCoordinates& loc, const AzimuthalCoordinates &pa_start)
{
	AzimuthalCoordinates pa = pa_start;

// Perform Newton iteration to obtain a better estimation for PA coordinates

	int i = 0;
	double diff = 1e10;
	Transformation t, t1, t2;
	bool diverge = false;

	while (i++ <= MAX_ITERATION && diff > tol)
	{
		getMisalignedPolarAxisTransformation(t, pa, loc);
		getMisalignedPolarAxisTransformation(t1,
				AzimuthalCoordinates(pa.alt + delta, pa.azi), loc);
		getMisalignedPolarAxisTransformation(t2,
				AzimuthalCoordinates(pa.alt, pa.azi + delta), loc);

		LocalEquatorialCoordinates star = applyMisalignment(t, star_ref),
				star1 = applyMisalignment(t1, star_ref), star2 =
						applyMisalignment(t2, star_ref);

		// Calculate Jacobian matrix. Everything should be divided by delta
		double j11 = star1.dec - star.dec, j12 = star2.dec - star.dec;
		double j21 = star1.ha - star.ha, j22 = star2.ha - star.ha;
		double det = j11 * j22 - j12 * j21;

		if (det == 0)
		{
			diverge = true;
			break;
		}

		// Newton's Method
		// Everything should be multiplied by delta
		double dp1 = -(j22 * (star.dec - star_meas.dec)
				- j12 * (star.ha - star_meas.ha)) / det;
		double dp2 = -(-j21 * (star.dec - star_meas.dec)
				+ j11 * (star.ha - star_meas.ha)) / det;

		// Update the coordinates
		pa.alt += dp1 * delta;
		pa.azi += dp2 * delta;

		diff = sqrt(dp1 * dp1 + dp2 * dp2) * delta; // calculate the difference
		debug_if(CM_DEBUG, "Iteration %i, %f\t%f\tdiff=%f\t %e, %e, %e, %e\n",
				i, pa.alt, pa.azi, diff, j11, j12, j21, j22);
	}
	if (diverge)
	{
		/// Do something
		debug_if(CM_DEBUG, "Diverge\n");
	}
	debug_if(CM_DEBUG, "Final delta: %.2e\n", diff);
	return pa;
}

IndexOffset CelestialMath::alignOneStarForOffset(
		const LocalEquatorialCoordinates& star_ref,
		const MountCoordinates& star_meas)
{
// Convert the reference star to Mount coordinates using the same pier side setting
	MountCoordinates star = localEquatorialToMount(star_ref, star_meas.side);
	return IndexOffset(star_meas.dec_delta - star.dec_delta,
			star_meas.ra_delta - star.ra_delta);
}

void CelestialMath::alignTwoStars(const LocalEquatorialCoordinates star_ref[],
		const LocalEquatorialCoordinates star_meas[],
		const LocationCoordinates& loc, AzimuthalCoordinates& pa,
		LocalEquatorialCoordinates& offset)
{
	int i = 0;
	double diff = 1e10;
	Transformation t, t1, t2;
	bool diverge = false;

// First try to get a rough estimate for offset to avoid divergence
//	getMisalignedPolarAxisTransformation(t, pa, loc);
//	offset = star_meas[0] - applyMisalignment(t, star_ref[0]);

	while (i++ <= MAX_ITERATION && diff > tol)
	{
		getMisalignedPolarAxisTransformation(t, pa, loc);
		getMisalignedPolarAxisTransformation(t1,
				AzimuthalCoordinates(pa.alt + delta, pa.azi), loc);
		getMisalignedPolarAxisTransformation(t2,
				AzimuthalCoordinates(pa.alt, pa.azi + delta), loc);

		// Transform both starts and add offset
		LocalEquatorialCoordinates star[2] =
		{ applyMisalignment(t, star_ref[0]) + offset, applyMisalignment(t,
				star_ref[1]) + offset };
		LocalEquatorialCoordinates star1[2] =
		{ applyMisalignment(t1, star_ref[0]) + offset, applyMisalignment(t1,
				star_ref[1]) + offset };
		LocalEquatorialCoordinates star2[2] =
		{ applyMisalignment(t2, star_ref[0]) + offset, applyMisalignment(t2,
				star_ref[1]) + offset };

		// Calculate Jacobian matrix. Everything should be divided by delta
		// The 4x4 matrix has a special structure, it can be blocked as
		// J1  I
		// J2  I
		// Where J1 and J2 are the 2x2 Jacobians as in alignOneStar and I is 2x2 identity matrix
		// Calculate J1=J
		double j11 = star1[0].dec - star[0].dec, j12 = star2[0].dec
				- star[0].dec;
		double j21 = star1[0].ha - star[0].ha, j22 = star2[0].ha - star[0].ha;
		// Calculate J2=K
		double k11 = star1[1].dec - star[1].dec, k12 = star2[1].dec
				- star[1].dec;
		double k21 = star1[1].ha - star[1].ha, k22 = star2[1].ha - star[1].ha;
		double det = (j11 - k11) * (j22 - k22) - (j12 - k12) * (j21 - k21); // det(J) = det(J1-J2)
		if (det == 0)
		{
			diverge = true;
			break;
		}

		// Calculate invert of J1-J2
		double i11 = (j22 - k22) / det, i12 = -(j12 - k12) / det;
		double i21 = -(j21 - k21) / det, i22 = (j11 - k11) / det;
		// Calculate J2(J1-J2)^-1
		double l11 = k11 * i11 + k12 * i21, l12 = k11 * i12 + k12 * i22;
		double l21 = k21 * i11 + k22 * i21, l22 = k21 * i12 + k22 * i22;

		// Calculate F1, F2, F3, F4
		double f1 = star[0].dec - star_meas[0].dec;
		double f2 = star[0].ha - star_meas[0].ha;
		double f3 = star[1].dec - star_meas[1].dec;
		double f4 = star[1].ha - star_meas[1].ha;

		// Newton's Method - Calculate J^-1 * F
		// dp1,2 should be multiplied by delta
		double dp1 = i11 * f1 + i12 * f2 - i11 * f3 - i12 * f4;
		double dp2 = i21 * f1 + i22 * f2 - i21 * f3 - i22 * f4;
		double dp3 = -l11 * f1 - l12 * f2 + (1 + l11) * f3 + l12 * f4;
		double dp4 = -l21 * f1 - l22 * f2 + l21 * f3 + (1 + l22) * f4;

		// Update the coordinates
		pa.alt += -dp1 * delta;
		pa.azi += -dp2 * delta;
		offset.dec += -dp3;
		offset.ha += -dp4;

		diff = sqrt(f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4); // calculate the difference
		debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\tdiff=%f\t %e %e\n", i,
				pa.alt, pa.azi, offset.dec, offset.ha, diff, det,
				det * (i11 * i22 - i12 * i21));
	}
	if (diverge)
	{
		/// Do something
		debug_if(CM_DEBUG, "Diverge\n");
	}
	debug_if(CM_DEBUG, "Final delta: %.2e\n", diff);
}

void CelestialMath::alignTwoStars(const LocalEquatorialCoordinates star_ref[],
		const MountCoordinates star_meas[], const LocationCoordinates& loc,
		AzimuthalCoordinates& pa, IndexOffset& offset, bool &diverge)
{
// Initialize the PA and offset
	pa.alt = loc.lat;
	pa.azi = 0;
	offset = IndexOffset(0, 0);

	int i = 0;
	double residue = 1e10;
	Transformation t, t1, t2;
	diverge = false;

	while (i++ <= MAX_ITERATION && residue > tol)
	{
		getMisalignedPolarAxisTransformation(t, pa, loc);
		getMisalignedPolarAxisTransformation(t1,
				AzimuthalCoordinates(pa.alt + delta, pa.azi), loc);
		getMisalignedPolarAxisTransformation(t2,
				AzimuthalCoordinates(pa.alt, pa.azi + delta), loc);

		// Transform both starts and add offset
		MountCoordinates star[2] =
		{ localEquatorialToMount(applyMisalignment(t, star_ref[0]),
				star_meas[0].side) + offset, localEquatorialToMount(
				applyMisalignment(t, star_ref[1]), star_meas[1].side) + offset };
		MountCoordinates star1[2] =
				{ localEquatorialToMount(applyMisalignment(t1, star_ref[0]),
						star_meas[0].side) + offset, localEquatorialToMount(
						applyMisalignment(t1, star_ref[1]), star_meas[1].side)
						+ offset };
		MountCoordinates star2[2] =
				{ localEquatorialToMount(applyMisalignment(t2, star_ref[0]),
						star_meas[0].side) + offset, localEquatorialToMount(
						applyMisalignment(t2, star_ref[1]), star_meas[1].side)
						+ offset };

		// Calculate Jacobian matrix. Everything should be divided by delta
		// The 4x4 matrix has a special structure, it can be blocked as
		// J1  I
		// J2  I
		// Where J1 and J2 are the 2x2 Jacobians as in alignOneStar and I is 2x2 identity matrix
		// Calculate J1=J
		double j11 = star1[0].dec_delta - star[0].dec_delta, j12 =
				star2[0].dec_delta - star[0].dec_delta;
		double j21 = star1[0].ra_delta - star[0].ra_delta, j22 =
				star2[0].ra_delta - star[0].ra_delta;
		// Calculate J2=K
		double k11 = star1[1].dec_delta - star[1].dec_delta, k12 =
				star2[1].dec_delta - star[1].dec_delta;
		double k21 = star1[1].ra_delta - star[1].ra_delta, k22 =
				star2[1].ra_delta - star[1].ra_delta;
		double det = (j11 - k11) * (j22 - k22) - (j12 - k12) * (j21 - k21); // det(J) = det(J1-J2)
		if (det == 0)
		{
			diverge = true;
			break;
		}

		// Calculate invert of J1-J2
		double i11 = (j22 - k22) / det, i12 = -(j12 - k12) / det;
		double i21 = -(j21 - k21) / det, i22 = (j11 - k11) / det;
		// Calculate J2(J1-J2)^-1
		double l11 = k11 * i11 + k12 * i21, l12 = k11 * i12 + k12 * i22;
		double l21 = k21 * i11 + k22 * i21, l22 = k21 * i12 + k22 * i22;

		// Calculate F1, F2, F3, F4
		double f1 = star[0].dec_delta - star_meas[0].dec_delta;
		double f2 = star[0].ra_delta - star_meas[0].ra_delta;
		double f3 = star[1].dec_delta - star_meas[1].dec_delta;
		double f4 = star[1].ra_delta - star_meas[1].ra_delta;

		// Newton's Method - Calculate J^-1 * F
		// dp1,2 should be multiplied by delta
		double dp1 = i11 * f1 + i12 * f2 - i11 * f3 - i12 * f4;
		double dp2 = i21 * f1 + i22 * f2 - i21 * f3 - i22 * f4;
		double dp3 = -l11 * f1 - l12 * f2 + (1 + l11) * f3 + l12 * f4;
		double dp4 = -l21 * f1 - l22 * f2 + l21 * f3 + (1 + l22) * f4;

		// Update the coordinates
		pa.alt += -dp1 * delta;
		pa.azi += -dp2 * delta;
		offset.dec_off += -dp3;
		offset.ra_off += -dp4;

		residue = sqrt(f1 * f1 + f2 * f2 + f3 * f3 + f4 * f4); // calculate the difference
		debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\tdiff=%f\t %e %e\n", i,
				pa.alt, pa.azi, offset.dec_off, offset.ra_off, residue, det,
				det * (i11 * i22 - i12 * i21));
	}
	if (diverge)
	{
		/// Do something
		debug_if(CM_DEBUG, "Diverge\n");
	}
	debug_if(CM_DEBUG, "Final delta: %.2e\n", residue);
}

static double jac[20][5]; // can maximally hold 10 stars
static double jacjac[5][5]; // J'J
static double invj[5][5];

static void get_corrected_stars(const int N, LocalEquatorialCoordinates stars[],
		const LocalEquatorialCoordinates star_ref[],
		const LocationCoordinates& loc, const AzimuthalCoordinates& pa,
		const LocalEquatorialCoordinates& offset, double cone)
{
	static Transformation t;
	CelestialMath::getMisalignedPolarAxisTransformation(t, pa, loc);
	for (int i = 0; i < N; i++)
	{
		stars[i] = CelestialMath::applyConeError(
				CelestialMath::applyMisalignment(t, star_ref[i]), cone)
				+ offset;
	}
}

static void fill_jacobian(const int N, const int j,
		LocalEquatorialCoordinates stars0[],
		LocalEquatorialCoordinates stars1[], const double &dd)
{
	for (int i = 0; i < N; i++)
	{
		jac[i * 2][j] = (stars1[i].dec - stars0[i].dec) / dd;
		jac[i * 2 + 1][j] = (stars1[i].ha - stars0[i].ha) / dd;
	}
}

static double det33(int a1, int a2, int a3, int b1, int b2, int b3)
{
	return jacjac[a1][b1] * jacjac[a2][b2] * jacjac[a3][b3]
			+ jacjac[a1][b2] * jacjac[a2][b3] * jacjac[a3][b1]
			+ jacjac[a1][b3] * jacjac[a2][b1] * jacjac[a3][b2]
			- jacjac[a1][b1] * jacjac[a2][b3] * jacjac[a3][b2]
			- jacjac[a1][b2] * jacjac[a2][b1] * jacjac[a3][b3]
			- jacjac[a1][b3] * jacjac[a2][b2] * jacjac[a3][b1];
}

static double det44(int a1, int a2, int a3, int a4, int b1, int b2, int b3,
		int b4)
{
	return jacjac[a1][b1] * det33(a2, a3, a4, b2, b3, b4)
			- jacjac[a1][b2] * det33(a2, a3, a4, b1, b3, b4)
			+ jacjac[a1][b3] * det33(a2, a3, a4, b1, b2, b4)
			- jacjac[a1][b4] * det33(a2, a3, a4, b1, b2, b3);
}

static void invert()
{
	invj[0][0] = det44(1, 2, 3, 4, 1, 2, 3, 4);
	invj[1][0] = -det44(1, 2, 3, 4, 0, 2, 3, 4);
	invj[2][0] = det44(1, 2, 3, 4, 0, 1, 3, 4);
	invj[3][0] = -det44(1, 2, 3, 4, 0, 1, 2, 4);
	invj[4][0] = det44(1, 2, 3, 4, 0, 1, 2, 3);

	double det55 = invj[0][0] * jacjac[0][0] + invj[1][0] * jacjac[0][1]
			+ invj[2][0] * jacjac[0][2] + invj[3][0] * jacjac[0][3]
			+ invj[4][0] * jacjac[0][4];
	double idet55 = 1.0 / det55;

	invj[0][0] *= idet55;
	invj[1][0] *= idet55;
	invj[2][0] *= idet55;
	invj[3][0] *= idet55;
	invj[4][0] *= idet55;

	invj[0][1] = -det44(0, 2, 3, 4, 1, 2, 3, 4) * idet55;
	invj[1][1] = det44(0, 2, 3, 4, 0, 2, 3, 4) * idet55;
	invj[2][1] = -det44(0, 2, 3, 4, 0, 1, 3, 4) * idet55;
	invj[3][1] = det44(0, 2, 3, 4, 0, 1, 2, 4) * idet55;
	invj[4][1] = -det44(0, 2, 3, 4, 0, 1, 2, 3) * idet55;

	invj[0][2] = det44(0, 1, 3, 4, 1, 2, 3, 4) * idet55;
	invj[1][2] = -det44(0, 1, 3, 4, 0, 2, 3, 4) * idet55;
	invj[2][2] = det44(0, 1, 3, 4, 0, 1, 3, 4) * idet55;
	invj[3][2] = -det44(0, 1, 3, 4, 0, 1, 2, 4) * idet55;
	invj[4][2] = det44(0, 1, 3, 4, 0, 1, 2, 3) * idet55;

	invj[0][3] = -det44(0, 1, 2, 4, 1, 2, 3, 4) * idet55;
	invj[1][3] = det44(0, 1, 2, 4, 0, 2, 3, 4) * idet55;
	invj[2][3] = -det44(0, 1, 2, 4, 0, 1, 3, 4) * idet55;
	invj[3][3] = det44(0, 1, 2, 4, 0, 1, 2, 4) * idet55;
	invj[4][3] = -det44(0, 1, 2, 4, 0, 1, 2, 3) * idet55;

	invj[0][4] = det44(0, 1, 2, 3, 1, 2, 3, 4) * idet55;
	invj[1][4] = -det44(0, 1, 2, 3, 0, 2, 3, 4) * idet55;
	invj[2][4] = det44(0, 1, 2, 3, 0, 1, 3, 4) * idet55;
	invj[3][4] = -det44(0, 1, 2, 3, 0, 1, 2, 4) * idet55;
	invj[4][4] = det44(0, 1, 2, 3, 0, 1, 2, 3) * idet55;
}

static inline double sqr(double x)
{
	return x * x;
}

void CelestialMath::alignNStars(const int N,
		const LocalEquatorialCoordinates star_ref[],
		const LocalEquatorialCoordinates star_meas[],
		const LocationCoordinates& loc, AzimuthalCoordinates& pa,
		LocalEquatorialCoordinates& offset, double& cone)
{
	if (N == 2)
	{
		alignTwoStars(star_ref, star_meas, loc, pa, offset);
		cone = 0;
		return;
	}
	if (N <= 1)
	{
		return;
	}

// Assuming the cone error is not huge, we should be fairly close to the local minimum
	int i = 0;
	double residue = 1e10;
	LocalEquatorialCoordinates stars0[N], stars1[N];
	double dp[5];
	double f[20];

	while (i++ < MAX_ITERATION_OPTIMIZATION && residue > tol)
	{
		// Calulate Jacobian
		get_corrected_stars(N, stars0, star_ref, loc, pa, offset, cone);
		/*Vary pa.alt*/
		get_corrected_stars(N, stars1, star_ref, loc,
				AzimuthalCoordinates(pa.alt + delta, pa.azi), offset, cone);
		fill_jacobian(N, 0, stars0, stars1, delta);
		/*Vary pa.azi*/
		get_corrected_stars(N, stars1, star_ref, loc,
				AzimuthalCoordinates(pa.alt, pa.azi + delta), offset, cone);
		fill_jacobian(N, 1, stars0, stars1, delta);
		/*Vary offset.dec*/
		get_corrected_stars(N, stars1, star_ref, loc, pa,
				LocalEquatorialCoordinates(offset.dec + delta, offset.ha),
				cone);
		fill_jacobian(N, 2, stars0, stars1, delta);
		/*Vary offset.ha*/
		get_corrected_stars(N, stars1, star_ref, loc, pa,
				LocalEquatorialCoordinates(offset.dec, offset.ha + delta),
				cone);
		fill_jacobian(N, 3, stars0, stars1, delta);
		/*Vary cone*/
		get_corrected_stars(N, stars1, star_ref, loc, pa, offset, cone + delta);
		fill_jacobian(N, 4, stars0, stars1, delta);

		// The Jacobian is now filled. It is 2*N rows and 5 columns
		// Gauss-Newton method: x_n - x_(n-1) = - (J'J)^-1J' * f_(n-1)
		// 1. Matrix multiplication
		int p, q, r;

		for (p = 0; p < 5; p++)
		{
			for (q = 0; q < 5; q++)
			{
				double s = 0;
				for (r = 0; r < 2 * N; r++)
					s += jac[r][p] * jac[r][q];
				jacjac[p][q] = s;
			}
		}

		// 2. Matrix inversion
		invert();

		// 3. Calculate f_(n-1)
		double newresidue = 0;
		for (p = 0; p < N; p++)
		{
			f[2 * p] = stars0[p].dec - star_meas[p].dec;
			f[2 * p + 1] = stars0[p].ha - star_meas[p].ha;
			newresidue += sqr(f[2 * p]) + sqr(f[2 * p + 1]);
		}
		newresidue = sqrt(newresidue);
		// 4. Matrix multiplication
		for (p = 0; p < 5; p++)
		{
			double s = 0;
			for (q = 0; q < 5; q++)
			{
				for (r = 0; r < 2 * N; r++)
				{
					s += invj[p][q] * jac[r][q] * f[r];
				}
			}
			dp[p] = -s;
		}
		// 5. Apply the correction
		pa.alt += dp[0];
		pa.azi += dp[1];
		offset.dec += dp[2];
		offset.ha += dp[3];
		cone += dp[4];

		if (newresidue >= residue - tol)
		{
			debug_if(CM_DEBUG, "Converged.\n");
			break;
		}
		else
		{
			residue = newresidue;
		}
		debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\t%f\tr=%f\n", i,
				pa.alt, pa.azi, offset.dec, offset.ha, cone, residue);
	}

	debug_if(CM_DEBUG, "Final result: %f\t%f\t%f\t%f\t%f\tr=%f\n", pa.alt,
			pa.azi, offset.dec, offset.ha, cone, residue);
}

static void get_corrected_stars(const int N, MountCoordinates stars[],
		const LocalEquatorialCoordinates star_ref[],
		const MountCoordinates star_meas[], const LocationCoordinates& loc,
		const AzimuthalCoordinates& pa, const IndexOffset& offset, double cone)
{
	static Transformation t;
	CelestialMath::getMisalignedPolarAxisTransformation(t, pa, loc);
	for (int i = 0; i < N; i++)
	{
		// Misalign, apply cone error, and transform to mount coordinates using the same pier side as in the measured stars
		stars[i] = CelestialMath::localEquatorialToMount(
				CelestialMath::applyConeError(
						CelestialMath::applyMisalignment(t, star_ref[i]), cone),
				star_meas[i].side) + offset;
	}
}

static void fill_jacobian(const int N, const int j, MountCoordinates stars0[],
		MountCoordinates stars1[], const double &dd)
{
	for (int i = 0; i < N; i++)
	{
		jac[i * 2][j] = (stars1[i].dec_delta - stars0[i].dec_delta) / dd;
		jac[i * 2 + 1][j] = (stars1[i].ra_delta - stars0[i].ra_delta) / dd;
	}
}

void CelestialMath::alignNStars(const int N,
		const LocalEquatorialCoordinates star_ref[],
		const MountCoordinates star_meas[], const LocationCoordinates& loc,
		AzimuthalCoordinates& pa, IndexOffset& offset, double& cone,
		bool &diverge)
{
	if (N == 2)
	{
		alignTwoStars(star_ref, star_meas, loc, pa, offset, diverge);
		cone = 0;
		return;
	}
	if (N <= 1)
	{
		return;
	}

// Assuming the cone error is not huge, we should be fairly close to the local minimum
	int i = 0;
	double residue = 1e10;
	MountCoordinates stars0[N], stars1[N];
	double dp[5];
	double f[20];

	diverge = true;

	while (i++ < MAX_ITERATION_OPTIMIZATION && residue > tol)
	{
		// Calulate Jacobian
		get_corrected_stars(N, stars0, star_ref, star_meas, loc, pa, offset,
				cone);
		/*Vary pa.alt*/
		get_corrected_stars(N, stars1, star_ref, star_meas, loc,
				AzimuthalCoordinates(pa.alt + delta, pa.azi), offset, cone);
		fill_jacobian(N, 0, stars0, stars1, delta);
		/*Vary pa.azi*/
		get_corrected_stars(N, stars1, star_ref, star_meas, loc,
				AzimuthalCoordinates(pa.alt, pa.azi + delta), offset, cone);
		fill_jacobian(N, 1, stars0, stars1, delta);
		/*Vary offset.dec*/
		get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa,
				IndexOffset(offset.dec_off + delta, offset.ra_off), cone);
		fill_jacobian(N, 2, stars0, stars1, delta);
		/*Vary offset.ha*/
		get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa,
				IndexOffset(offset.dec_off, offset.ra_off + delta), cone);
		fill_jacobian(N, 3, stars0, stars1, delta);
		/*Vary cone*/
		get_corrected_stars(N, stars1, star_ref, star_meas, loc, pa, offset,
				cone + delta);
		fill_jacobian(N, 4, stars0, stars1, delta);

		// The Jacobian is now filled. It is 2*N rows and 5 columns
		// Gauss-Newton method: x_n - x_(n-1) = - (J'J)^-1J' * f_(n-1)
		// 1. Matrix multiplication
		int p, q, r;

		for (p = 0; p < 5; p++)
		{
			for (q = 0; q < 5; q++)
			{
				double s = 0;
				for (r = 0; r < 2 * N; r++)
					s += jac[r][p] * jac[r][q];
				jacjac[p][q] = s;
			}
		}

		// 2. Matrix inversion
		invert();

		// 3. Calculate f_(n-1)
		double newresidue = 0;
		for (p = 0; p < N; p++)
		{
			f[2 * p] = stars0[p].dec_delta - star_meas[p].dec_delta;
			f[2 * p + 1] = stars0[p].ra_delta - star_meas[p].ra_delta;
			newresidue += sqr(f[2 * p]) + sqr(f[2 * p + 1]);
		}
		newresidue = sqrt(newresidue);
		// 4. Matrix multiplication
		for (p = 0; p < 5; p++)
		{
			double s = 0;
			for (q = 0; q < 5; q++)
			{
				for (r = 0; r < 2 * N; r++)
				{
					s += invj[p][q] * jac[r][q] * f[r];
				}
			}
			dp[p] = -s;
		}
		// 5. Apply the correction
		pa.alt += dp[0];
		pa.azi += dp[1];
		offset.dec_off += dp[2];
		offset.ra_off += dp[3];
		cone += dp[4];

		if (newresidue >= residue - tol)
		{
			debug_if(CM_DEBUG, "Converged.\n");
			diverge = false;
			break;
		}
		else
		{
			residue = newresidue;
		}
		debug_if(CM_DEBUG, "Iteration %i, %f\t%f\t%f\t%f\t%f\tr=%f\n", i,
				pa.alt, pa.azi, offset.dec_off, offset.ra_off, cone, residue);
	}

	if (diverge)
	{
		debug_if(CM_DEBUG, "Diverged.\n");
	}

	debug_if(CM_DEBUG, "Final result: %f\t%f\t%f\t%f\t%f\tr=%f\n", pa.alt,
			pa.azi, offset.dec_off, offset.ra_off, cone, residue);

}

EqCalibration CelestialMath::align(const int N, const AlignmentStar stars[],
		const LocationCoordinates &loc, bool &diverge)
{
	EqCalibration calib;
	calib.pa.alt = loc.lat;
	if (N == 1)
	{
		calib.offset = alignOneStarForOffset(stars[0].star_ref_local(loc),
				stars[0].star_meas);
		diverge = false;
	}
	else
	{
		LocalEquatorialCoordinates star_ref[N];
		MountCoordinates star_meas[N];
		for (int i = 0; i < N; i++)
		{
			star_ref[i] = stars[i].star_ref_local(loc);
			star_meas[i] = stars[i].star_meas;
		}
		if (N == 2)
		{
			alignTwoStars(star_ref, star_meas, loc, calib.pa, calib.offset,
					diverge);
		}
		else
		{
			alignNStars(N, star_ref, star_meas, loc, calib.pa, calib.offset,
					calib.cone, diverge);
		}
	}
	calib.error = CelestialMath::alignmentError(N, stars, calib, loc);

	return calib;
}

double CelestialMath::parseHMSAngle(char* hms)
{
	char *h = strchr(hms, 'h');
	char *m = strchr(hms, 'm');
	char *s = strchr(hms, 's');
	if (h == NULL || m == NULL || s == NULL || !(h < m && m < s))
	{
		return NAN;
	}

	*h = '\0';
	*m = '\0';
	*s = '\0';

	char *tp;
	int hour = strtol(hms, &tp, 10);
	if (tp == hms)
	{
		return NAN;
	}
	int minute = strtol(h + 1, &tp, 10);
	if (tp == h + 1)
	{
		return NAN;
	}
	double second = strtod(m + 1, &tp);
	if (tp == m + 1)
	{
		return NAN;
	}

	if (!(hour >= 0 && hour <= 23 && minute >= 0 && minute <= 59 && second >= 0
			&& second <= 60))
	{
		return NAN;
	}

	return remainder((hour + minute / 60.0 + second / 3600.0) * 15, 360);
}

double CelestialMath::parseDMSAngle(char* dms)
{
	char *d = strchr(dms, 'd');
	char *m = strchr(dms, 'm');
	char *s = strchr(dms, 's');
	if (d == NULL || m == NULL || s == NULL || !(d < m && m < s))
	{
		return NAN;
	}

	*d = '\0';
	*m = '\0';
	*s = '\0';

	char *tp;
	int degree = strtol(dms, &tp, 10);
	if (tp == dms)
	{
		return NAN;
	}
	int arcminute = strtol(d + 1, &tp, 10);
	if (tp == d + 1)
	{
		return NAN;
	}
	double arcsecond = strtod(m + 1, &tp);
	if (tp == m + 1)
	{
		return NAN;
	}

	if (!(degree >= -180.0 && degree <= 180.0 && arcminute >= 0
			&& arcminute <= 59 && arcsecond >= 0 && arcsecond <= 60))
	{
		return NAN;
	}

	return remainder((degree + arcminute / 60.0 + arcsecond / 3600.0), 360);
}

double CelestialMath::kingRate(EquatorialCoordinates eq,
		LocationCoordinates loc, double time)
{
	LocalEquatorialCoordinates leq = CelestialMath::equatorialToLocalEquatorial(
			eq, time, loc);
//	AzimuthalCoordinates ac = CelestialMath::localEquatorialToAzimuthal(leq,
//			loc);
	double cosLat = cos(loc.lat * DEGREE);
	double sinLat = sin(loc.lat * DEGREE);
	double cotLat = cosLat / sinLat;
	double cosDec = cos(eq.dec * DEGREE);
	double sinDec = sin(eq.dec * DEGREE);
	double tanDec = sinDec / cosDec;
	double cosHA = cos(leq.ha * DEGREE);
	double kingMpD = (1436.46
			+ 0.4
					* (cosLat / cosDec
							* (cosLat * cosDec + sinLat * sinDec * cosHA)
							/ pow(sinLat * sinDec + cosLat * cosDec * cosHA,
									2.0) - cotLat * tanDec * cosHA));
	return 6.0 / kingMpD;
}

double CelestialMath::alignmentError(const int N, const AlignmentStar stars[],
		const EqCalibration &calib, const LocationCoordinates& loc)
{
	static Transformation t;
	CelestialMath::getMisalignedPolarAxisTransformation(t, calib.pa, loc);
	double r = 0;
	for (int i = 0; i < N; i++)
	{
		// Misalign, apply cone error, and transform to mount coordinates using the same pier side as in the measured stars
		MountCoordinates mc = CelestialMath::localEquatorialToMount(
				CelestialMath::applyConeError(
						CelestialMath::applyMisalignment(t,
								CelestialMath::equatorialToLocalEquatorial(
										stars[i].star_ref, stars[i].timestamp,
										loc)), calib.cone),
				stars[i].star_meas.side) + calib.offset;

		r += sqr(mc.ra_delta - stars[i].star_meas.ra_delta)
				+ sqr(mc.dec_delta - stars[i].star_meas.dec_delta);
	}
	return sqrt(r);
}