mirror
/
RS-tracker
kopia lustrzana https://github.com/rs1729/RS
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Wahl der Lsg nahe Oberflaeche

dump
Zilog80 2015-10-30 14:18:02 +01:00
rodzic cea339868b
commit 43cf3d2f5a
1 zmienionych plików z 54 dodań i 54 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

108
rs92/nav_gps.c
Wyświetl plik

@ -615,77 +615,74 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
A[1][2] = 2.0*z13; A[1][2] = 2.0*z13;
A[2][2] = 2.0*z14; A[2][2] = 2.0*z14;
tmp1 = A[1][1]*A[2][2] - A[2][1]*A[1][2]; tmp1 = A[1][1]*A[2][2] - A[2][1]*A[1][2];
tmp2 = A[1][0]*A[2][2] - A[2][0]*A[1][2]; tmp2 = A[1][0]*A[2][2] - A[2][0]*A[1][2];
tmp3 = A[1][0]*A[2][1] - A[2][0]*A[1][1]; tmp3 = A[1][0]*A[2][1] - A[2][0]*A[1][1];
detA = A[0][0]*tmp1 - A[0][1]*tmp2 + A[0][2]*tmp3; detA = A[0][0]*tmp1 - A[0][1]*tmp2 + A[0][2]*tmp3;
D[0][0] = tmp1; D[0][0] = tmp1;
D[1][0] = -tmp2; D[1][0] = -tmp2;
D[2][0] = tmp3; D[2][0] = tmp3;
D[0][1] = -A[0][1]*A[2][2] + A[2][1]*A[0][2]; D[0][1] = -A[0][1]*A[2][2] + A[2][1]*A[0][2];
D[1][1] = A[0][0]*A[2][2] - A[2][0]*A[0][2]; D[1][1] = A[0][0]*A[2][2] - A[2][0]*A[0][2];
D[2][1] = -A[0][0]*A[2][1] + A[2][0]*A[0][1]; D[2][1] = -A[0][0]*A[2][1] + A[2][0]*A[0][1];
D[0][2] = A[0][1]*A[1][2] - A[1][1]*A[0][2]; D[0][2] = A[0][1]*A[1][2] - A[1][1]*A[0][2];
D[1][2] = -A[0][0]*A[1][2] + A[1][0]*A[0][2]; D[1][2] = -A[0][0]*A[1][2] + A[1][0]*A[0][2];
D[2][2] = A[0][0]*A[1][1] - A[1][0]*A[0][1]; D[2][2] = A[0][0]*A[1][1] - A[1][0]*A[0][1];
c1 = (D[0][0]*p21 + D[0][1]*p31 + D[0][2]*p41) * 2.0 / detA; c1 = (D[0][0]*p21 + D[0][1]*p31 + D[0][2]*p41) * 2.0 / detA;
c2 = (D[1][0]*p21 + D[1][1]*p31 + D[1][2]*p41) * 2.0 / detA; c2 = (D[1][0]*p21 + D[1][1]*p31 + D[1][2]*p41) * 2.0 / detA;
c3 = (D[2][0]*p21 + D[2][1]*p31 + D[2][2]*p41) * 2.0 / detA; c3 = (D[2][0]*p21 + D[2][1]*p31 + D[2][2]*p41) * 2.0 / detA;
f1 = (D[0][0]*k1 + D[0][1]*k2 + D[0][2]*k3) / detA; f1 = (D[0][0]*k1 + D[0][1]*k2 + D[0][2]*k3) / detA;
f2 = (D[1][0]*k1 + D[1][1]*k2 + D[1][2]*k3) / detA; f2 = (D[1][0]*k1 + D[1][1]*k2 + D[1][2]*k3) / detA;
f3 = (D[2][0]*k1 + D[2][1]*k2 + D[2][2]*k3) / detA; f3 = (D[2][0]*k1 + D[2][1]*k2 + D[2][2]*k3) / detA;
m1 = c1*c1 + c2*c2 + c3*c3 - 1.0; m1 = c1*c1 + c2*c2 + c3*c3 - 1.0;
m2 = -2.0*( c1*f1 + c2*f2 + c3*f3 ); m2 = -2.0*( c1*f1 + c2*f2 + c3*f3 );
m3 = f1*f1 + f2*f2 + f3*f3; m3 = f1*f1 + f2*f2 + f3*f3;
tmp1 = m2*m2 - 4.0*m1*m3; tmp1 = m2*m2 - 4.0*m1*m3;
if ( tmp1 < 0 ) { // not good, there is no solution if ( tmp1 < 0 ) { // not good, there is no solution
return 0; //FALSE return 0; //FALSE
} }
d1 = ( -m2 - sqrt( tmp1 ) ) * 0.5 / m1; // +-Reihenfolge vertauscht d1 = ( -m2 - sqrt( tmp1 ) ) * 0.5 / m1; // +-Reihenfolge vertauscht
d2 = ( -m2 + sqrt( tmp1 ) ) * 0.5 / m1; d2 = ( -m2 + sqrt( tmp1 ) ) * 0.5 / m1;
// two solutions // two solutions
s1.x = c1*d1 - f1 + x1; s1.x = c1*d1 - f1 + x1;
s1.y = c2*d1 - f2 + y1; s1.y = c2*d1 - f2 + y1;
s1.z = c3*d1 - f3 + z1; s1.z = c3*d1 - f3 + z1;
s2.x = c1*d2 - f1 + x1; s2.x = c1*d2 - f1 + x1;
s2.y = c2*d2 - f2 + y1; s2.y = c2*d2 - f2 + y1;
s2.z = c3*d2 - f3 + z1; s2.z = c3*d2 - f3 + z1;
ecef2elli( s1.x, s1.y, s1.z, &lat1, &lon1, &alt1 ); tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z );
tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z );
*latitude = lat1;
*longitude = lon1;
*height = alt1;
tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z );
tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z );
// choose the correct solution based // choose the correct solution based
// on whichever solution is closest to // on whichever solution is closest to
// the Earth's surface // the Earth's surface
tmp1 = fabs( tmp1 - 6371000.0 ); tmp1 = fabs( tmp1 - 6371000.0 );
tmp2 = fabs( tmp2 - 6371000.0 ); tmp2 = fabs( tmp2 - 6371000.0 );
// geht manchmal schief
// wenn beide Werte nahe Erdoberflaeche // nur (tmp2 < tmp1) geht manchmal schief
// ungefaehre Position kann aus den Positionen der empfangen Sats abgeleitet werden if ( tmp2 < tmp1 && tmp1 >= 60000 ) { // swap solutions
if ( tmp2 < tmp1 /*alt2 < alt1 && alt2 > -1500*/ && tmp1 < 60000 ) stmp = s1; s1 = s2; s2 = stmp; // s1 = s2;
{ dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2;
x0 = (x1+x2+x3+x4)/4.0; }
y0 = (y1+y2+y3+y4)/4.0; else if ( tmp2 < 60000 ) { // interessant wenn tmp1<tmp2<60k oder tmp2<tmp1<60k,
z0 = (z1+z2+z3+z4)/4.0; x0 = (x1+x2+x3+x4)/4.0; // d.h. beide Werte nahe Erdoberflaeche;
y0 = (y1+y2+y3+y4)/4.0; // ungefaehre Position kann aus den Positionen
z0 = (z1+z2+z3+z4)/4.0; // der empfangen Sats abgeleitet werden
ecef2elli( x0, y0, z0, &latS, &lonS, &altS ); ecef2elli( x0, y0, z0, &latS, &lonS, &altS );
ecef2elli( s1.x, s1.y, s1.z, &lat1, &lon1, &alt1 );
ecef2elli( s2.x, s2.y, s2.z, &lat2, &lon2, &alt2 ); ecef2elli( s2.x, s2.y, s2.z, &lat2, &lon2, &alt2 );
d2_1 = sqrt( (latS-lat1)*(latS-lat1) + (lonS-lon1)*(lonS-lon1) ); d2_1 = sqrt( (latS-lat1)*(latS-lat1) + (lonS-lon1)*(lonS-lon1) );
@ -694,12 +691,15 @@ int NAV_ClosedFormPositionSolution_FromPseudorange(
if ( d2_2 < d2_1 ) { if ( d2_2 < d2_1 ) {
stmp = s1; s1 = s2; s2 = stmp; // s1 = s2; stmp = s1; s1 = s2; s2 = stmp; // s1 = s2;
dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2; dtmp = d1; d1 = d2; d2 = dtmp; // d1 = d2;
*latitude = lat2;
*longitude = lon2;
*height = alt2;
} }
} }
ecef2elli( s1.x, s1.y, s1.z, &lat1, &lon1, &alt1 );
*latitude = lat1;
*longitude = lon1;
*height = alt1;
*rx_clock_bias = d1; *rx_clock_bias = d1;
pos_ecef[0] = s1.x; pos_ecef[0] = s1.x;