467 lines
17 KiB
C
467 lines
17 KiB
C
#include "rtklib.h"
|
||
|
||
#define LAMBDA_P0 0.80
|
||
#define MIN_SAT 6
|
||
|
||
#define LOOPMAX 10000 /* maximum count of search loop */
|
||
#define SGN(x) ((x) <= 0.0 ? -1.0 : 1.0)
|
||
#define ROUND(x) (floor((x) + 0.5))
|
||
#define SWAP(x, y) \
|
||
do \
|
||
{ \
|
||
double tmp_; \
|
||
tmp_ = x; \
|
||
x = y; \
|
||
y = tmp_; \
|
||
} while (0)
|
||
|
||
static int parsearch(rtk_t *rtk, int n, const double *zhat, const double *Qzhat, const double *Z,
|
||
const double *L, const double *D, int m, double *F, double *s);
|
||
|
||
/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
|
||
static int LD(int n, const double *Q, double *L, double *D)
|
||
{
|
||
int i, j, k, info = 0;
|
||
double a, *A = mat(n, n);
|
||
|
||
memcpy(A, Q, sizeof(double) * n * n);
|
||
for (i = n - 1; i >= 0; i--)
|
||
{
|
||
if ((D[i] = A[i + i * n]) <= 0.0)
|
||
{
|
||
info = -1;
|
||
break;
|
||
}
|
||
a = sqrt(D[i]);
|
||
for (j = 0; j <= i; j++)
|
||
L[i + j * n] = A[i + j * n] / a;
|
||
for (j = 0; j <= i - 1; j++)
|
||
for (k = 0; k <= j; k++)
|
||
A[j + k * n] -= L[i + k * n] * L[i + j * n];
|
||
for (j = 0; j <= i; j++)
|
||
L[i + j * n] /= L[i + i * n];
|
||
}
|
||
free(A);
|
||
if (info)
|
||
fprintf(stderr, "%s : LD factorization error\n", __FILE__);
|
||
return info;
|
||
}
|
||
/* integer gauss transformation ----------------------------------------------*/
|
||
static void gauss(int n, double *L, double *Z, int i, int j)
|
||
{
|
||
int k, mu;
|
||
|
||
if ((mu = (int)ROUND(L[i + j * n])) != 0)
|
||
{
|
||
for (k = i; k < n; k++)
|
||
L[k + n * j] -= (double)mu * L[k + i * n];
|
||
for (k = 0; k < n; k++)
|
||
Z[k + n * j] -= (double)mu * Z[k + i * n];
|
||
}
|
||
}
|
||
/* permutations --------------------------------------------------------------*/
|
||
static void perm(int n, double *L, double *D, int j, double del, double *Z)
|
||
{
|
||
int k;
|
||
double eta, lam, a0, a1;
|
||
|
||
eta = D[j] / del;
|
||
lam = D[j + 1] * L[j + 1 + j * n] / del;
|
||
D[j] = eta * D[j + 1];
|
||
D[j + 1] = del;
|
||
for (k = 0; k <= j - 1; k++)
|
||
{
|
||
a0 = L[j + k * n];
|
||
a1 = L[j + 1 + k * n];
|
||
L[j + k * n] = -L[j + 1 + j * n] * a0 + a1;
|
||
L[j + 1 + k * n] = eta * a0 + lam * a1;
|
||
}
|
||
L[j + 1 + j * n] = lam;
|
||
for (k = j + 2; k < n; k++)
|
||
SWAP(L[k + j * n], L[k + (j + 1) * n]);
|
||
for (k = 0; k < n; k++)
|
||
SWAP(Z[k + j * n], Z[k + (j + 1) * n]);
|
||
}
|
||
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
|
||
static void reduction(int n, double *L, double *D, double *Z)
|
||
{
|
||
int i, j, k;
|
||
double del;
|
||
|
||
j = n - 2;
|
||
k = n - 2;
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB><EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˼·<CBBC><C2B7>
|
||
while (j >= 0)
|
||
{
|
||
//<2F><><EFBFBD>ڵ<EFBFBD>k+1<><31>k+2<><32>...<2E><>n-2<>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><D8B2><EFBFBD>û<EFBFBD>б<EFBFBD><D0B1><EFBFBD>һ<EFBFBD>ε<EFBFBD><CEB5><EFBFBD><EFBFBD>任Ӱ<E4BBBB>죬
|
||
//<2F><><EFBFBD><EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD>Ե<EFBFBD>0,1<><31>...<2E><>k-1<><31>k<EFBFBD>н<EFBFBD><D0BD>н<EFBFBD><D0BD><EFBFBD><EFBFBD><EFBFBD>
|
||
if (j <= k)
|
||
for (i = j + 1; i < n; i++)
|
||
gauss(n, L, Z, i, j); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>п<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>зǶԽ<C7B6><D4BD><EFBFBD>Ԫ<EFBFBD>ش<EFBFBD><D8B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ν<EFBFBD><CEBD><EFBFBD><EFBFBD><EFBFBD>
|
||
del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
|
||
if (del + 1E-6 < D[j + 1])
|
||
{ /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD>任compared considering numerical error */
|
||
perm(n, L, D, j, del, Z); //<2F><><EFBFBD><EFBFBD><EFBFBD>任
|
||
k = j;
|
||
j = n - 2; //<2F><><EFBFBD>ɵ<EFBFBD><C9B5><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD>´<EFBFBD><C2B4><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>п<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD>н<EFBFBD><D0BD><EFBFBD><EFBFBD>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>k<EFBFBD><6B>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>һ<EFBFBD>ν<EFBFBD><CEBD>й<EFBFBD><D0B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
}
|
||
else
|
||
j--;
|
||
}
|
||
}
|
||
/* modified lambda (mlambda) search (ref. [2]) -------------------------------
|
||
* args : n I number of float parameters
|
||
* m I number of fixed solution
|
||
L,D I transformed covariance matrix
|
||
zs I transformed double-diff phase biases
|
||
zn O fixed solutions
|
||
s O sum of residuals for fixed solutions */
|
||
static int search(int n, int m, const double *L, const double *D,
|
||
const double *zs, double *zn, double *s)
|
||
{
|
||
int i, j, k, c, nn = 0, imax = 0;
|
||
double newdist, maxdist = 1E99, y; // maxdist<73><74><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Բ<EFBFBD>뾶
|
||
double *S = zeros(n, n), *dist = mat(n, 1), *zb = mat(n, 1), *z = mat(n, 1), *step = mat(n, 1);
|
||
|
||
k = n - 1;
|
||
dist[k] = 0.0; // k<><6B>ʾ<EFBFBD><CABE>ǰ<EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>㣨n-1<><31><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
|
||
zb[k] = zs[k]; //<2F><>zn
|
||
z[k] = ROUND(zb[k]); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δȡ<CEB4><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>step<65><70>¼z[k]<5D><><EFBFBD><EFBFBD><EFBFBD>ỹ<EFBFBD><E1BBB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
y = zb[k] - z[k];
|
||
step[k] = SGN(y); /* step towards closest integer */
|
||
for (c = 0; c < LOOPMAX; c++)
|
||
{
|
||
newdist = dist[k] + y * y / D[k]; /* newdist=sum(((z(j)-zb(j))^2/d(j))) */
|
||
if (newdist < maxdist)
|
||
{ //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>ۻ<EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵС<D6B5>ڵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Բ<EFBFBD>뾶
|
||
/* Case 1: move down <20><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۻ<EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ */
|
||
if (k != 0)
|
||
{
|
||
dist[--k] = newdist; //<2F><>¼<EFBFBD>µ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD>ۻ<EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5>dist[k]<5D><>ʾ<EFBFBD>˵<EFBFBD>k,k+1,...,n-1<><31><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
for (i = 0; i <= k; i++)
|
||
S[k + i * n] = S[k + 1 + i * n] + (z[k + 1] - zb[k + 1]) * L[k + 1 + i * n];
|
||
zb[k] = zs[k] + S[k + k * n]; //<2F><><EFBFBD><EFBFBD>Zk<5A><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>k<EFBFBD><6B><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD>Ȳ<EFBFBD><C8B2><EFBFBD><EFBFBD>ı<EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
z[k] = ROUND(zb[k]); /* next valid integer <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δȡ<CEB4><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEA3BB>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD>ỹ<EFBFBD><E1BBB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
|
||
y = zb[k] - z[k];
|
||
step[k] = SGN(y);
|
||
}
|
||
/* Case 2: store the found candidate and try next valid integer <20><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>㣬<EFBFBD><E3A3AC>ζ<EFBFBD><CEB6><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><D0B2><EFBFBD><EFBFBD>ۻ<EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
|
||
else
|
||
{
|
||
// nnΪ<6E><CEAA>ǰ<EFBFBD><C7B0>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>mΪ<6D><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>Ĺ̶<C4B9><CCB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ2<CEAA><32><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>Ҫһ<D2AA><D2BB><EFBFBD><EFBFBD><EFBFBD>Ž⼰һ<E2BCB0><D2BB><EFBFBD><EFBFBD><EFBFBD>Ž<EFBFBD>
|
||
// s<><73>¼<EFBFBD><C2BC>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5>imax<61><78>¼֮ǰ<D6AE><C7B0>ѡ<EFBFBD><D1A1><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
if (nn < m)
|
||
{ //<2F><><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB>
|
||
/* store the first m initial points */
|
||
if (nn == 0 || newdist > s[imax])
|
||
imax = nn; //<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5>֮ǰ<D6AE><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD>imaxʹs[imax]ָ<><D6B8><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>о<EFBFBD><D0BE>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ
|
||
for (i = 0; i < n; i++)
|
||
zn[i + nn * n] = z[i]; // zn<7A><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>к<EFBFBD>ѡ<EFBFBD><D1A1>
|
||
s[nn++] = newdist; // s<><73>¼<EFBFBD><C2BC>ǰĿ<C7B0>꺯<EFBFBD><EABAAF>ֵnewdist<73><74><EFBFBD><EFBFBD><EFBFBD>Ӽӵ<D3BC>ǰ<EFBFBD><C7B0>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD>nn
|
||
}
|
||
else
|
||
{ //<2F><><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰzn<7A><6E><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD><EFBFBD>ѡ<EFBFBD>⣩
|
||
if (newdist < s[imax])
|
||
{ //<2F><> <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ <20><> s<>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ С
|
||
for (i = 0; i < n; i++)
|
||
zn[i + imax * n] = z[i]; //<2F>õ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>滻zn<7A>о<EFBFBD><D0BE>нϴ<D0BD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD>Ľ<EFBFBD>
|
||
s[imax] = newdist; //<2F>õ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD>滻s<E6BBBB>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ
|
||
for (i = imax = 0; i < m; i++)
|
||
if (s[imax] < s[i])
|
||
imax = i; //<2F><><EFBFBD><EFBFBD>imax<61><78>֤imaxʼ<78><CABC>ָ<EFBFBD><D6B8>s<EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ
|
||
}
|
||
maxdist = s[imax]; //<2F>õ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5><EFBFBD>³<EFBFBD><C2B3><EFBFBD>Բ<EFBFBD>뾶
|
||
}
|
||
z[0] += step[0]; /* next valid integer<65>ڵ<EFBFBD>һ<EFBFBD>㣬ȡ<E3A3AC><C8A1>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD>Ȳ<EFBFBD><C8B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>м<EFBFBD><D0BC>㣨<EFBFBD><E3A3A8>zbΪ5.3<EFBFBD><EFBFBD><EFBFBD><EFBFBD>zȡֵ˳<EFBFBD><EFBFBD>Ϊ5,6,4,7<><37>...<2E><> */
|
||
y = zb[0] - z[0];
|
||
step[0] = -step[0] - SGN(step[0]);
|
||
}
|
||
}
|
||
/* Case 3: exit or move up<75><70><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>ۻ<EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>ڵ<EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Բ<EFBFBD>뾶 */
|
||
else
|
||
{
|
||
if (k == n - 1)
|
||
break; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0>Ϊ<EFBFBD><CEAA>n-1<>㣬<EFBFBD><E3A3AC>ζ<EFBFBD>ź<EFBFBD><C5BA><EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC>㶼<EFBFBD>ᳬ<EFBFBD><E1B3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD>뾶<EFBFBD><EBBEB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>
|
||
else
|
||
{ //<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD>㲻<EFBFBD>ǵ<EFBFBD>n-1<><31>
|
||
k++; /* move up <20>˺<EFBFBD>һ<EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD>ӵ<EFBFBD>k<EFBFBD><6B><EFBFBD>˵<EFBFBD><CBB5><EFBFBD>k+1<><31>*/
|
||
z[k] += step[k]; /* next valid integer <20><><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>ѡ<EFBFBD><D1A1>*/
|
||
y = zb[k] - z[k];
|
||
step[k] = -step[k] - SGN(step[k]);
|
||
}
|
||
}
|
||
}
|
||
// <20><>s<EFBFBD>е<EFBFBD>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5>zn<7A>еĺ<D0B5>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>s<EFBFBD><73>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵΪ<D6B5><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><D7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// RTKLIB<49><42><EFBFBD><EFBFBD><EFBFBD>տ<EFBFBD><D5BF>Եõ<D4B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>Ž<EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD>Ž⣬<C5BD><E2A3AC><EFBFBD><EFBFBD>zn<7A>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6>Ŀ<EFBFBD>꺯<EFBFBD><EABAAF>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>s<EFBFBD><73>
|
||
for (i = 0; i < m - 1; i++)
|
||
{ /* sort by s */
|
||
for (j = i + 1; j < m; j++)
|
||
{
|
||
if (s[i] < s[j])
|
||
continue;
|
||
SWAP(s[i], s[j]);
|
||
for (k = 0; k < n; k++)
|
||
SWAP(zn[k + i * n], zn[k + j * n]);
|
||
}
|
||
}
|
||
free(S);
|
||
free(dist);
|
||
free(zb);
|
||
free(z);
|
||
free(step);
|
||
|
||
if (c >= LOOPMAX)
|
||
{
|
||
fprintf(stderr, "%s : search loop count overflow\n", __FILE__);
|
||
return -2;
|
||
}
|
||
return 0;
|
||
}
|
||
static float my_normcdf(float a) // <20><>ȡ<EFBFBD><C8A1>̬<EFBFBD>ֲ<EFBFBD>CDF
|
||
{
|
||
float h, l, r;
|
||
const float SQRT_HALF_HI = 0x1.6a09e6p-01f; // 7.07106769e-1
|
||
const float SQRT_HALF_LO = 0x1.9fcef4p-27f; // 1.21016175e-8
|
||
/* clamp input as normcdf(x) is either 0 or 1 asymptotically */
|
||
if (fabsf(a) > 14.171875f)
|
||
a = (a < 0.0f) ? -14.171875f : 14.171875f;
|
||
h = fmaf(-SQRT_HALF_HI, a, -SQRT_HALF_LO * a);
|
||
l = fmaf(SQRT_HALF_LO, a, fmaf(SQRT_HALF_HI, a, h));
|
||
r = erfcf(h);
|
||
if (h > 0.0f)
|
||
r = fmaf(2.0f * h * l, r, r);
|
||
return 0.5f * r;
|
||
}
|
||
|
||
static void nmatgetkmat_RD(const double *nmat, int n1, int n2, int m1, int m2, double *mmat)
|
||
{
|
||
// double *mmat;
|
||
int i, j;
|
||
// mmat = mat(m1, m2);
|
||
//<2F>Ӵ<EFBFBD><D3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡС<C8A1><D0A1><EFBFBD><EFBFBD>
|
||
for (i = 0; i < m1; i++)
|
||
for (j = 0; j < m2; j++)
|
||
{
|
||
*(mmat + i * m2 + j) = *(nmat + (n1 - m1) * n2 + (n2 - m2) + i * n2 + j);
|
||
}
|
||
|
||
// return mmat;
|
||
}
|
||
|
||
static void nmatgetkmat_RU(const double *nmat, int n1, int n2, int m1, int m2, double *mmat)
|
||
{
|
||
// double *mmat;
|
||
int i, j;
|
||
// mmat = mat(m1, m2);
|
||
//<2F>Ӵ<EFBFBD><D3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡС<C8A1><D0A1><EFBFBD><EFBFBD>
|
||
for (i = 0; i < m1; i++)
|
||
for (j = 0; j < m2; j++)
|
||
{
|
||
*(mmat + i * m2 + j) = *(nmat + (n2 - m2) + i * n2 + j);
|
||
}
|
||
|
||
// return mmat;
|
||
}
|
||
/* parlambda ------------------------------
|
||
* integer least-square estimation. reduction is performed by lambda (ref.[1]),
|
||
* and search by mlambda (ref.[2]).
|
||
* args :
|
||
* rtk_t* rtk I
|
||
* int n I number of float parameters
|
||
* int m I number of fixed solutions
|
||
* double *a I float parameters (n x 1) (double-diff phase biases)
|
||
* double *Q I covariance matrix of float parameters (n x n)
|
||
* double *F O fixed solutions (n x m)
|
||
* double *s O sum of squared residulas of fixed solutions (1 x m)
|
||
* return : status (0:ok,other:error)
|
||
* notes : matrix stored by column-major order (fortran convension)
|
||
*-----------------------------------------------------------------------------*/
|
||
extern int parlambda(rtk_t *rtk, int n, int m, const double *a, const double *Q, double *F,
|
||
double *s)
|
||
{
|
||
int info;
|
||
double *L, *D, *Z, *z, *E, *Qt, *Qzhat;
|
||
if (n <= 0 || m <= 0)
|
||
return -1;
|
||
L = zeros(n, n);
|
||
D = mat(n, 1);
|
||
Z = eye(n);
|
||
z = mat(n, 1);
|
||
E = mat(n, m);
|
||
|
||
/* LD (lower diaganol) factorization (Q=L'*diag(D)*L) */
|
||
if (!(info = LD(n, Q, L, D)))
|
||
{
|
||
|
||
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) */
|
||
reduction(n, L, D, Z);
|
||
matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z); /* z=Z'*a */
|
||
|
||
/*par search*/
|
||
// Qz = Z'*Q*Z
|
||
Qt = mat(n, n);
|
||
Qzhat = mat(n, n);
|
||
matmul("TN", n, n, n, 1.0, Z, Q, 0.0, Qt); // Qt = Z'*Q
|
||
matmul("NN", n, n, n, 1.0, Qt, Z, 0.0, Qzhat); // Qz =QtZ
|
||
if (!(info = parsearch(rtk, n, z, Qzhat, Z, L, D, 2, F, s)))
|
||
{
|
||
;
|
||
}
|
||
free(Qt);
|
||
free(Qzhat);
|
||
}
|
||
free(L);
|
||
free(D);
|
||
free(Z);
|
||
free(z);
|
||
free(E);
|
||
return info;
|
||
}
|
||
|
||
/*parsearch
|
||
*by LongRui Peng&ZiWen Qu from ZJUT
|
||
args:
|
||
rtk_t *rtk I
|
||
int n I number of float parameter
|
||
const double *zhat I decorrelated float ambiguities
|
||
const double *Qzhat I variance-covariance matrix of decorrelated float ambiguities
|
||
const double *Z I Z-matrix from decorrel
|
||
L,D I lower-triangular and diagonal matrix from LtDL-decomposition of Qzhat
|
||
m I Number of requested integer candidate vectors [DEFAULT=2]
|
||
double *F O fixed solutions (n x m)
|
||
double *s O sum of squared residulas of fixed solutions (1 x m)
|
||
|
||
*/
|
||
static int parsearch(rtk_t *rtk, int n, const double *zhat, const double *Qzhat, const double *Z,
|
||
const double *L, const double *D, int m, double *F, double *s)
|
||
|
||
{
|
||
int info = 0, p, k, i, j;
|
||
float Ps = 1; /* Ps <20><>cumulative success rate P0:Minimum required sucess rate [DEFAULT=0.8] */
|
||
|
||
double *zk_n, *Lk_n, *Dk_n, *z1_fix, *Q11, *Q12, *Qp, *z2_fix, *z_t1, *z_t2, *z_fix;
|
||
for (k = n; k > 2; k--)
|
||
{
|
||
if (Ps > LAMBDA_P0)
|
||
{
|
||
Ps = Ps * (2 * my_normcdf((float)(1 / (2 * sqrt(20 * D[k - 1])))) - 1);
|
||
}
|
||
else
|
||
break;
|
||
}
|
||
// Lk_n: new matrix formed by [k:n,k:n] of L
|
||
// Dk_n: new diag formed by [k:n] row of D
|
||
// zk_n: new column formed by [k:n] element of zhat
|
||
// Lk_n: new matrix formed by [k:n,k:n] of L
|
||
// Dk_n: new diag formed by [k:n] row of D
|
||
// zk_n: new column formed by [k:n] element of zhat
|
||
p = n - k + 1;
|
||
|
||
while (p >= MIN_SAT)
|
||
{
|
||
Dk_n = mat(p, 1);
|
||
zk_n = mat(p, 1);
|
||
Lk_n = mat(p, p);
|
||
z1_fix = mat(p, m);
|
||
j = k - 1;
|
||
for (i = 0; i < p; i++)
|
||
{
|
||
Dk_n[i] = D[j];
|
||
zk_n[i] = zhat[j];
|
||
j++;
|
||
}
|
||
nmatgetkmat_RD(L, n, n, p, p, Lk_n);
|
||
|
||
if (!(info = search(p, m, Lk_n, Dk_n, zk_n, z1_fix, s))) /* returns 0 if no error */
|
||
{
|
||
if (s[0] <= 0.0 || s[1] / s[0] >= rtk->sol.thres) // Ratio test
|
||
{
|
||
Q12 = mat(k - 1, p);
|
||
Q11 = mat(p, p);
|
||
Qp = mat(k - 1, p);
|
||
z2_fix = mat(k - 1, m);
|
||
z_t1 = mat(p, m);
|
||
z_t2 = mat(k - 1, m);
|
||
z_fix = mat(n, m);
|
||
// first k - 1 ambiguities are adjusted based on correlation with the fixed ambiguities
|
||
nmatgetkmat_RD(Qzhat, n, n, p, p, Q11);
|
||
nmatgetkmat_RU(Qzhat, n, n, k - 1, p, Q12);
|
||
matinv(Q11, p);
|
||
matmul("NN", k - 1, p, p, 1.0, Q12, Q11, 0.0, Qp); // Qp = Q12 / Q11;
|
||
for (i = 0; i < 2 * p; i++)
|
||
{
|
||
if (i < p)
|
||
z_t1[i] = zk_n[i] - z1_fix[i];
|
||
else
|
||
z_t1[i] = zk_n[i - p] - z1_fix[i];
|
||
}
|
||
|
||
matmul("NN", k - 1, m, p, 1.0, Qp, z_t1, 0.0, z_t2); // zt_2=Qp(zk_n - z1_fix)
|
||
/*z2_fix = z2_float - Qp(zk_n - z1_fix);*/
|
||
for (i = 0; i < 2 * k - 2; i++)
|
||
{
|
||
if (i < k - 1)
|
||
z2_fix[i] = zhat[i] - z_t2[i];
|
||
else
|
||
z2_fix[i] = zhat[i - k + 1] - z_t2[i];
|
||
}
|
||
// for (i = 0; i < 2 * k - 2; i++)
|
||
//{
|
||
// z2_fix[i] = ROUND(z2_fix[i]);
|
||
// }
|
||
/* z_fix = [z2_fix, z1_fix]; */
|
||
for (i = 0; i < 2 * n; i++)
|
||
{
|
||
if (i < k - 1)
|
||
z_fix[i] = z2_fix[i];
|
||
else if (i < n)
|
||
z_fix[i] = z1_fix[i - k + 1];
|
||
else if (i < k - 1 + n)
|
||
z_fix[i] = z2_fix[i - n + k - 1];
|
||
else
|
||
z_fix[i] = z1_fix[i - 2 * k + 2];
|
||
}
|
||
|
||
/* <20><><EFBFBD><EFBFBD><EFBFBD>¿ռ<C2BF><D5BC>й̶<D0B9><CCB6><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB>˫<EFBFBD><CBAB>ģ<EFBFBD><C4A3><EFBFBD>ȿռ<C8BF><D5BC><EFBFBD> */
|
||
info = solve("T", Z, z_fix, n, m, F); /* F=Z'\E */
|
||
free(Dk_n);
|
||
free(zk_n);
|
||
free(Lk_n);
|
||
free(z1_fix);
|
||
free(Q12);
|
||
free(Q11);
|
||
free(Qp);
|
||
free(z2_fix);
|
||
free(z_t1);
|
||
free(z_t2);
|
||
free(z_fix);
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
trace(3, "PAR ratio test failed ! ,remove the ambiguity with the largest variance \n");
|
||
p--;
|
||
k++;
|
||
free(Dk_n);
|
||
free(zk_n);
|
||
free(Lk_n);
|
||
free(z1_fix);
|
||
}
|
||
}
|
||
}
|
||
if (p < MIN_SAT) // set the minimum number of part ambiguities 6
|
||
{
|
||
trace(3, "PAR lack sat! Now:%d output float solution... \n", p);
|
||
z_fix = mat(n, m);
|
||
if (!(info = search(n, m, L, D, zhat, z_fix, s)))
|
||
{ /* returns 0 if no error */
|
||
|
||
/* <20><><EFBFBD><EFBFBD><EFBFBD>¿ռ<C2BF><D5BC>й̶<D0B9><CCB6><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>任<EFBFBD><E4BBBB>˫<EFBFBD><CBAB>ģ<EFBFBD><C4A3><EFBFBD>ȿռ<C8BF><D5BC><EFBFBD> */
|
||
info = solve("T", Z, z_fix, n, m, F); /* F=Z'\E */
|
||
}
|
||
free(z_fix);
|
||
info = 0;
|
||
}
|
||
|
||
return info;
|
||
} |