Debugging GPS is hard. There is very little external information or tools you can use to verify your work as you go.
For my test data-set I am getting all space vehicles in the correct area of their orbits, and all time-stamps look to be correct.
The solution to the ‘four unknowns’ also converges to a point – it is in the correct country, but the altitude is all wrong:
Location of Space Vehicle 1 is ( 10718280.453, -11194350.324, 21437533.145) @ 466730.48644070 Location of Space Vehicle 29 is ( 12521753.091, 8721571.916, 21816482.385) @ 466730.48738122 Location of Space Vehicle 30 is ( 18407387.316, 6499170.025, 17615903.809) @ 466730.48924957 Location of Space Vehicle 31 is ( 20795459.146, -8979584.903, 14229278.406) @ 466730.48564003 Solved is ( 4,777,302, -45,951, 6,231,361) @ 0.009795 (alt 7,852,043.479301)
Lat/Lon/Alt ( 52.2914, -0.5511, -11,488,221.25)
Obviously the Altitude is wrong – it is 11,000 km up in space (half way to the space vehicles)! But it is at least over the correct rough geographic area.
I’m really unsure how to proceed. I’ll have to put my thinking cap on.
My first guess is that the correction from the Space Vehicle’s local clock to GPS time is wrong. I’ll have to look into it in great detail tonight.
Here is my amended version of Andrew Holme’s solution routine, which has been converted to C, and had a few minor interface tweeks. This “struct Location” are just ‘x’,’y’,’z’ and ‘time’ values, as doubles. You may also need to add a few definitions for constants (as this is snipped out of a bigger source file).
Once I get this working I intend to write my own solver….
//////////////////////////////////////////////////////////////////////////
// Homemade GPS Receiver
// Copyright (C) 2013 Andrew Holme
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
// http://www.aholme.co.uk/GPS/Main.htm
//
// Note this version has been moved to 'C' by Mike Field for testing his
// attempt at building a GPS receiver
//////////////////////////////////////////////////////////////////////////
#include <memory.h>
#define MAX_ITER 20
#define WGS84_A (6378137.0)
#define WGS84_F_INV (298.257223563)
#define WGS84_B (6356752.31424518)
#define WGS84_E2 (0.00669437999014132)
#define OMEGA_E (7.2921151467e-5) /* Earth's rotation rate */
///////////////////////////////////////////////////////////////////////////////////////////////
#define NUM_CHANS 10
int A_solve(int chans, struct Location *sv, struct Location *p) {
int i, j, r, c;
double t_tx[NUM_CHANS]; // Clock replicas in seconds since start of week
double x_sv[NUM_CHANS], y_sv[NUM_CHANS], z_sv[NUM_CHANS];
double t_pc; // Uncorrected system time when clock replica snapshots taken
double t_rx; // Corrected GPS time
double dPR[NUM_CHANS]; // Pseudo range error
double jac[NUM_CHANS][4], ma[4][4], mb[4][4], mc[4][NUM_CHANS], md[4];
double weight[NUM_CHANS];
p->x = p->y = p->z = p->time = t_pc = 0.0;
for (i=0; i<chans; i++) {
weight[i] = 1.0;
x_sv[i] = sv[i].x;
y_sv[i] = sv[i].y;
z_sv[i] = sv[i].z;
t_tx[i] = sv[i].time;
t_pc += sv[i].time;
}
// Approximate starting value for receiver clock
t_pc = t_pc/chans + 75e-3;
// Iterate to user xyzt solution using Taylor Series expansion:
double err_mag;
for(j=0; j<MAX_ITER; j++) {
t_rx = t_pc - p->time;
for (i=0; i<chans; i++) {
// Convert SV position to ECI coords (20.3.3.4.3.3.2)
double theta = (t_tx[i] - t_rx) * OMEGA_E;
double x_sv_eci = x_sv[i]*cos(theta) - y_sv[i]*sin(theta);
double y_sv_eci = x_sv[i]*sin(theta) + y_sv[i]*cos(theta);
double z_sv_eci = z_sv[i];
// Geometric range (20.3.3.4.3.4)
double gr = sqrt(pow(p->x - x_sv_eci, 2) +
pow(p->y - y_sv_eci, 2) +
pow(p->z - z_sv_eci, 2));
dPR[i] = SPEED_OF_LIGHT*(t_rx - t_tx[i]) - gr;
jac[i][0] = (p->x - x_sv_eci) / gr;
jac[i][1] = (p->y - y_sv_eci) / gr;
jac[i][2] = (p->z - z_sv_eci) / gr;
jac[i][3] = SPEED_OF_LIGHT;
}
// ma = transpose(H) * W * H
for (r=0; r<4; r++)
for (c=0; c<4; c++) {
ma[r][c] = 0;
for (i=0; i<chans; i++) ma[r][c] += jac[i][r]*weight[i]*jac[i][c];
}
double determinant =
ma[0][3]*ma[1][2]*ma[2][1]*ma[3][0] - ma[0][2]*ma[1][3]*ma[2][1]*ma[3][0] - ma[0][3]*ma[1][1]*ma[2][2]*ma[3][0] + ma[0][1]*ma[1][3]*ma[2][2]*ma[3][0]+
ma[0][2]*ma[1][1]*ma[2][3]*ma[3][0] - ma[0][1]*ma[1][2]*ma[2][3]*ma[3][0] - ma[0][3]*ma[1][2]*ma[2][0]*ma[3][1] + ma[0][2]*ma[1][3]*ma[2][0]*ma[3][1]+
ma[0][3]*ma[1][0]*ma[2][2]*ma[3][1] - ma[0][0]*ma[1][3]*ma[2][2]*ma[3][1] - ma[0][2]*ma[1][0]*ma[2][3]*ma[3][1] + ma[0][0]*ma[1][2]*ma[2][3]*ma[3][1]+
ma[0][3]*ma[1][1]*ma[2][0]*ma[3][2] - ma[0][1]*ma[1][3]*ma[2][0]*ma[3][2] - ma[0][3]*ma[1][0]*ma[2][1]*ma[3][2] + ma[0][0]*ma[1][3]*ma[2][1]*ma[3][2]+
ma[0][1]*ma[1][0]*ma[2][3]*ma[3][2] - ma[0][0]*ma[1][1]*ma[2][3]*ma[3][2] - ma[0][2]*ma[1][1]*ma[2][0]*ma[3][3] + ma[0][1]*ma[1][2]*ma[2][0]*ma[3][3]+
ma[0][2]*ma[1][0]*ma[2][1]*ma[3][3] - ma[0][0]*ma[1][2]*ma[2][1]*ma[3][3] - ma[0][1]*ma[1][0]*ma[2][2]*ma[3][3] + ma[0][0]*ma[1][1]*ma[2][2]*ma[3][3];
// mb = inverse(ma) = inverse(transpose(H)*W*H)
mb[0][0] = (ma[1][2]*ma[2][3]*ma[3][1] - ma[1][3]*ma[2][2]*ma[3][1] + ma[1][3]*ma[2][1]*ma[3][2] - ma[1][1]*ma[2][3]*ma[3][2] - ma[1][2]*ma[2][1]*ma[3][3] + ma[1][1]*ma[2][2]*ma[3][3]) / determinant;
mb[0][1] = (ma[0][3]*ma[2][2]*ma[3][1] - ma[0][2]*ma[2][3]*ma[3][1] - ma[0][3]*ma[2][1]*ma[3][2] + ma[0][1]*ma[2][3]*ma[3][2] + ma[0][2]*ma[2][1]*ma[3][3] - ma[0][1]*ma[2][2]*ma[3][3]) / determinant;
mb[0][2] = (ma[0][2]*ma[1][3]*ma[3][1] - ma[0][3]*ma[1][2]*ma[3][1] + ma[0][3]*ma[1][1]*ma[3][2] - ma[0][1]*ma[1][3]*ma[3][2] - ma[0][2]*ma[1][1]*ma[3][3] + ma[0][1]*ma[1][2]*ma[3][3]) / determinant;
mb[0][3] = (ma[0][3]*ma[1][2]*ma[2][1] - ma[0][2]*ma[1][3]*ma[2][1] - ma[0][3]*ma[1][1]*ma[2][2] + ma[0][1]*ma[1][3]*ma[2][2] + ma[0][2]*ma[1][1]*ma[2][3] - ma[0][1]*ma[1][2]*ma[2][3]) / determinant;
mb[1][0] = (ma[1][3]*ma[2][2]*ma[3][0] - ma[1][2]*ma[2][3]*ma[3][0] - ma[1][3]*ma[2][0]*ma[3][2] + ma[1][0]*ma[2][3]*ma[3][2] + ma[1][2]*ma[2][0]*ma[3][3] - ma[1][0]*ma[2][2]*ma[3][3]) / determinant;
mb[1][1] = (ma[0][2]*ma[2][3]*ma[3][0] - ma[0][3]*ma[2][2]*ma[3][0] + ma[0][3]*ma[2][0]*ma[3][2] - ma[0][0]*ma[2][3]*ma[3][2] - ma[0][2]*ma[2][0]*ma[3][3] + ma[0][0]*ma[2][2]*ma[3][3]) / determinant;
mb[1][2] = (ma[0][3]*ma[1][2]*ma[3][0] - ma[0][2]*ma[1][3]*ma[3][0] - ma[0][3]*ma[1][0]*ma[3][2] + ma[0][0]*ma[1][3]*ma[3][2] + ma[0][2]*ma[1][0]*ma[3][3] - ma[0][0]*ma[1][2]*ma[3][3]) / determinant;
mb[1][3] = (ma[0][2]*ma[1][3]*ma[2][0] - ma[0][3]*ma[1][2]*ma[2][0] + ma[0][3]*ma[1][0]*ma[2][2] - ma[0][0]*ma[1][3]*ma[2][2] - ma[0][2]*ma[1][0]*ma[2][3] + ma[0][0]*ma[1][2]*ma[2][3]) / determinant;
mb[2][0] = (ma[1][1]*ma[2][3]*ma[3][0] - ma[1][3]*ma[2][1]*ma[3][0] + ma[1][3]*ma[2][0]*ma[3][1] - ma[1][0]*ma[2][3]*ma[3][1] - ma[1][1]*ma[2][0]*ma[3][3] + ma[1][0]*ma[2][1]*ma[3][3]) / determinant;
mb[2][1] = (ma[0][3]*ma[2][1]*ma[3][0] - ma[0][1]*ma[2][3]*ma[3][0] - ma[0][3]*ma[2][0]*ma[3][1] + ma[0][0]*ma[2][3]*ma[3][1] + ma[0][1]*ma[2][0]*ma[3][3] - ma[0][0]*ma[2][1]*ma[3][3]) / determinant;
mb[2][2] = (ma[0][1]*ma[1][3]*ma[3][0] - ma[0][3]*ma[1][1]*ma[3][0] + ma[0][3]*ma[1][0]*ma[3][1] - ma[0][0]*ma[1][3]*ma[3][1] - ma[0][1]*ma[1][0]*ma[3][3] + ma[0][0]*ma[1][1]*ma[3][3]) / determinant;
mb[2][3] = (ma[0][3]*ma[1][1]*ma[2][0] - ma[0][1]*ma[1][3]*ma[2][0] - ma[0][3]*ma[1][0]*ma[2][1] + ma[0][0]*ma[1][3]*ma[2][1] + ma[0][1]*ma[1][0]*ma[2][3] - ma[0][0]*ma[1][1]*ma[2][3]) / determinant;
mb[3][0] = (ma[1][2]*ma[2][1]*ma[3][0] - ma[1][1]*ma[2][2]*ma[3][0] - ma[1][2]*ma[2][0]*ma[3][1] + ma[1][0]*ma[2][2]*ma[3][1] + ma[1][1]*ma[2][0]*ma[3][2] - ma[1][0]*ma[2][1]*ma[3][2]) / determinant;
mb[3][1] = (ma[0][1]*ma[2][2]*ma[3][0] - ma[0][2]*ma[2][1]*ma[3][0] + ma[0][2]*ma[2][0]*ma[3][1] - ma[0][0]*ma[2][2]*ma[3][1] - ma[0][1]*ma[2][0]*ma[3][2] + ma[0][0]*ma[2][1]*ma[3][2]) / determinant;
mb[3][2] = (ma[0][2]*ma[1][1]*ma[3][0] - ma[0][1]*ma[1][2]*ma[3][0] - ma[0][2]*ma[1][0]*ma[3][1] + ma[0][0]*ma[1][2]*ma[3][1] + ma[0][1]*ma[1][0]*ma[3][2] - ma[0][0]*ma[1][1]*ma[3][2]) / determinant;
mb[3][3] = (ma[0][1]*ma[1][2]*ma[2][0] - ma[0][2]*ma[1][1]*ma[2][0] + ma[0][2]*ma[1][0]*ma[2][1] - ma[0][0]*ma[1][2]*ma[2][1] - ma[0][1]*ma[1][0]*ma[2][2] + ma[0][0]*ma[1][1]*ma[2][2]) / determinant;
// mc = inverse(transpose(H)*W*H) * transpose(H)
for (r=0; r<4; r++)
for (c=0; c<chans; c++) {
mc[r][c] = 0;
for (i=0; i<4; i++) mc[r][c] += mb[r][i]*jac[c][i];
}
// md = inverse(transpose(H)*W*H) * transpose(H) * W * dPR
for (r=0; r<4; r++) {
md[r] = 0;
for (i=0; i<chans; i++) md[r] += mc[r][i]*weight[i]*dPR[i];
}
double dx = md[0];
double dy = md[1];
double dz = md[2];
double dt = md[3];
err_mag = sqrt(dx*dx + dy*dy + dz*dz);
if (err_mag<1.0) break;
printf("A_SOLVE %2i: %14g (%14g,%14g,%14g) %14g\n", j, err_mag, p->time, p->x, p->y, p->z);
p->x += dx;
p->y += dy;
p->z += dz;
p->time += dt;
}
printf("A_SOLVE %2i: %14g (%14g,%14g,%14g) %14g\n", j, err_mag, p->time, p->x, p->y, p->z);
return j;
}
///////////////////////////////////////////////////////////////////////////////////////////////
static void LatLonAlt(
double x_n, double y_n, double z_n,
double *lat, double *lon, double *alt) {
const double a = WGS84_A;
const double e2 = WGS84_E2;
const double p = sqrt(x_n*x_n + y_n*y_n);
*lon = 2.0 * atan2(y_n, x_n + p);
*lat = atan(z_n / (p * (1.0 - e2)));
*alt = 0.0;
for (;;) {
double tmp = *alt;
double N = a / sqrt(1.0 - e2*pow(sin(*lat),2));
*alt = p/cos(lat) - N;
*lat = atan(z_n / (p * (1.0 - e2*N/(N + *alt))));
if (fabs(*alt-tmp)<1e-3) break;
}
}
/*********************************************************************************************************/
GPS Demystified 