2017-04-24 22:48:13 +00:00
|
|
|
/*!
|
|
|
|
* \file rtklib_lambda.cc
|
|
|
|
* \brief Integer ambiguity resolution
|
|
|
|
* \authors <ul>
|
|
|
|
* <li> 2007-2008, T. Takasu
|
|
|
|
* <li> 2017, Javier Arribas
|
|
|
|
* <li> 2017, Carles Fernandez
|
|
|
|
* </ul>
|
|
|
|
*
|
|
|
|
* This is a derived work from RTKLIB http://www.rtklib.com/
|
|
|
|
* The original source code at https://github.com/tomojitakasu/RTKLIB is
|
|
|
|
* released under the BSD 2-clause license with an additional exclusive clause
|
|
|
|
* that does not apply here. This additional clause is reproduced below:
|
|
|
|
*
|
|
|
|
* " The software package includes some companion executive binaries or shared
|
|
|
|
* libraries necessary to execute APs on Windows. These licenses succeed to the
|
|
|
|
* original ones of these software. "
|
|
|
|
*
|
|
|
|
* Neither the executive binaries nor the shared libraries are required by, used
|
|
|
|
* or included in GNSS-SDR.
|
|
|
|
*
|
|
|
|
* -------------------------------------------------------------------------
|
|
|
|
* Copyright (C) 2007-2008, T. Takasu
|
|
|
|
* Copyright (C) 2017, Javier Arribas
|
|
|
|
* Copyright (C) 2017, Carles Fernandez
|
|
|
|
* All rights reserved.
|
|
|
|
*
|
|
|
|
* Redistribution and use in source and binary forms, with or without
|
|
|
|
* modification, are permitted provided that the following conditions are
|
|
|
|
* met:
|
|
|
|
*
|
|
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
|
|
* notice, this list of conditions and the following disclaimer.
|
|
|
|
*
|
|
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
|
|
* documentation and/or other materials provided with the distribution.
|
|
|
|
*
|
|
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
|
|
|
|
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
|
|
|
* HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
|
|
|
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
|
|
|
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
|
|
|
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
|
|
|
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
|
|
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
|
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
*
|
|
|
|
*----------------------------------------------------------------------------*/
|
|
|
|
|
|
|
|
#include "rtklib_lambda.h"
|
|
|
|
#include "rtklib_rtkcmn.h"
|
|
|
|
|
|
|
|
/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/
|
|
|
|
int LD(int n, const double *Q, double *L, double *D)
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
int i, j, k, info = 0;
|
|
|
|
double a, *A = mat(n, n);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
memcpy(A, Q, sizeof(double) * n * n);
|
|
|
|
for (i = n - 1; i >= 0; i--)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if ((D[i] = A[i + i * n]) <= 0.0)
|
|
|
|
{
|
|
|
|
info = -1;
|
|
|
|
break;
|
|
|
|
}
|
2017-04-24 22:48:13 +00:00
|
|
|
a = sqrt(D[i]);
|
2018-03-03 01:03:39 +00:00
|
|
|
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];
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
free(A);
|
2018-03-03 01:03:39 +00:00
|
|
|
if (info) fprintf(stderr, "%s : LD factorization error\n", __FILE__);
|
2017-04-24 22:48:13 +00:00
|
|
|
return info;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* integer gauss transformation ----------------------------------------------*/
|
|
|
|
void gauss(int n, double *L, double *Z, int i, int j)
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
int k, mu;
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if ((mu = (int)ROUND_LAMBDA(L[i + j * n])) != 0)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
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];
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* permutations --------------------------------------------------------------*/
|
|
|
|
void perm(int n, double *L, double *D, int j, double del, double *Z)
|
|
|
|
{
|
|
|
|
int k;
|
2018-03-03 01:03:39 +00:00
|
|
|
double eta, lam, a0, a1;
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
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++)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
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;
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
L[j + 1 + j * n] = lam;
|
|
|
|
for (k = j + 2; k < n; k++) SWAP_LAMBDA(L[k + j * n], L[k + (j + 1) * n]);
|
|
|
|
for (k = 0; k < n; k++) SWAP_LAMBDA(Z[k + j * n], Z[k + (j + 1) * n]);
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* lambda reduction (z=Z'*a, Qz=Z'*Q*Z=L'*diag(D)*L) (ref.[1]) ---------------*/
|
|
|
|
void reduction(int n, double *L, double *D, double *Z)
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
int i, j, k;
|
2017-04-24 22:48:13 +00:00
|
|
|
double del;
|
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
j = n - 2;
|
|
|
|
k = n - 2;
|
2017-04-24 22:48:13 +00:00
|
|
|
while (j >= 0)
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (j <= k)
|
|
|
|
for (i = j + 1; i < n; i++) gauss(n, L, Z, i, j);
|
|
|
|
del = D[j] + L[j + 1 + j * n] * L[j + 1 + j * n] * D[j + 1];
|
|
|
|
if (del + 1E-6 < D[j + 1])
|
2017-04-24 22:48:13 +00:00
|
|
|
{ /* compared considering numerical error */
|
2018-03-03 01:03:39 +00:00
|
|
|
perm(n, L, D, j, del, Z);
|
|
|
|
k = j;
|
|
|
|
j = n - 2;
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
else
|
|
|
|
j--;
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* modified lambda (mlambda) search (ref. [2]) -------------------------------*/
|
|
|
|
int search(int n, int m, const double *L, const double *D,
|
2018-03-03 01:03:39 +00:00
|
|
|
const double *zs, double *zn, double *s)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
int i, j, k, c, nn = 0, imax = 0;
|
|
|
|
double newdist, maxdist = 1E99, y;
|
|
|
|
double *S = zeros(n, n), *dist = mat(n, 1), *zb = mat(n, 1), *z = mat(n, 1), *step = mat(n, 1);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
k = n - 1;
|
|
|
|
dist[k] = 0.0;
|
2017-04-24 22:48:13 +00:00
|
|
|
zb[k] = zs[k];
|
|
|
|
z[k] = ROUND_LAMBDA(zb[k]);
|
2018-03-03 01:03:39 +00:00
|
|
|
y = zb[k] - z[k];
|
2017-04-24 22:48:13 +00:00
|
|
|
step[k] = SGN_LAMBDA(y);
|
2018-03-03 01:03:39 +00:00
|
|
|
for (c = 0; c < LOOPMAX; c++)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
newdist = dist[k] + y * y / D[k];
|
|
|
|
if (newdist < maxdist)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (k != 0)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
|
|
|
dist[--k] = newdist;
|
2018-03-03 01:03:39 +00:00
|
|
|
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];
|
|
|
|
z[k] = ROUND_LAMBDA(zb[k]);
|
|
|
|
y = zb[k] - z[k];
|
|
|
|
step[k] = SGN_LAMBDA(y);
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (nn < m)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (nn == 0 || newdist > s[imax]) imax = nn;
|
|
|
|
for (i = 0; i < n; i++) zn[i + nn * n] = z[i];
|
2017-04-24 22:48:13 +00:00
|
|
|
s[nn++] = newdist;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (newdist < s[imax])
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
for (i = 0; i < n; i++) zn[i + imax * n] = z[i];
|
2017-04-24 22:48:13 +00:00
|
|
|
s[imax] = newdist;
|
2018-03-03 01:03:39 +00:00
|
|
|
for (i = imax = 0; i < m; i++)
|
|
|
|
if (s[imax] < s[i]) imax = i;
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
maxdist = s[imax];
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
z[0] += step[0];
|
|
|
|
y = zb[0] - z[0];
|
|
|
|
step[0] = -step[0] - SGN_LAMBDA(step[0]);
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (k == n - 1)
|
|
|
|
break;
|
2017-04-24 22:48:13 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
k++;
|
2018-03-03 01:03:39 +00:00
|
|
|
z[k] += step[k];
|
|
|
|
y = zb[k] - z[k];
|
|
|
|
step[k] = -step[k] - SGN_LAMBDA(step[k]);
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
for (i = 0; i < m - 1; i++)
|
2017-04-24 22:48:13 +00:00
|
|
|
{ /* sort by s */
|
2018-03-03 01:03:39 +00:00
|
|
|
for (j = i + 1; j < m; j++)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
if (s[i] < s[j]) continue;
|
|
|
|
SWAP_LAMBDA(s[i], s[j]);
|
|
|
|
for (k = 0; k < n; k++) SWAP_LAMBDA(zn[k + i * n], zn[k + j * n]);
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
free(S);
|
|
|
|
free(dist);
|
|
|
|
free(zb);
|
|
|
|
free(z);
|
|
|
|
free(step);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
|
|
|
if (c >= LOOPMAX)
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
fprintf(stderr, "%s : search loop count overflow\n", __FILE__);
|
2017-04-24 22:48:13 +00:00
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* lambda/mlambda integer least-square estimation ------------------------------
|
|
|
|
* integer least-square estimation. reduction is performed by lambda (ref.[1]),
|
|
|
|
* and search by mlambda (ref.[2]).
|
|
|
|
* args : int n I number of float parameters
|
|
|
|
* int m I number of fixed solutions
|
|
|
|
* double *a I float parameters (n x 1)
|
|
|
|
* 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)
|
|
|
|
*-----------------------------------------------------------------------------*/
|
|
|
|
int lambda(int n, int m, const double *a, const double *Q, double *F,
|
2018-03-03 01:03:39 +00:00
|
|
|
double *s)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
|
|
|
int info;
|
2018-03-03 01:03:39 +00:00
|
|
|
double *L, *D, *Z, *z, *E;
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (n <= 0 || m <= 0) return -1;
|
|
|
|
L = zeros(n, n);
|
|
|
|
D = mat(n, 1);
|
2017-04-24 22:48:13 +00:00
|
|
|
Z = eye(n);
|
2018-03-03 01:03:39 +00:00
|
|
|
z = mat(n, 1);
|
|
|
|
E = mat(n, m);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
|
|
|
/* LD factorization */
|
2018-03-03 01:03:39 +00:00
|
|
|
if (!(info = LD(n, Q, L, D)))
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
|
|
|
/* lambda reduction */
|
2018-03-03 01:03:39 +00:00
|
|
|
reduction(n, L, D, Z);
|
|
|
|
matmul("TN", n, 1, n, 1.0, Z, a, 0.0, z); /* z=Z'*a */
|
2017-04-24 22:48:13 +00:00
|
|
|
|
|
|
|
/* mlambda search */
|
2018-03-03 01:03:39 +00:00
|
|
|
if (!(info = search(n, m, L, D, z, E, s)))
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
info = solve("T", Z, E, n, m, F); /* F=Z'\E */
|
2017-04-24 22:48:13 +00:00
|
|
|
}
|
|
|
|
}
|
2018-03-03 01:03:39 +00:00
|
|
|
free(L);
|
|
|
|
free(D);
|
|
|
|
free(Z);
|
|
|
|
free(z);
|
|
|
|
free(E);
|
2017-04-24 22:48:13 +00:00
|
|
|
return info;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* lambda reduction ------------------------------------------------------------
|
|
|
|
* reduction by lambda (ref [1]) for integer least square
|
|
|
|
* args : int n I number of float parameters
|
|
|
|
* double *Q I covariance matrix of float parameters (n x n)
|
|
|
|
* double *Z O lambda reduction matrix (n x n)
|
|
|
|
* return : status (0:ok,other:error)
|
|
|
|
*-----------------------------------------------------------------------------*/
|
|
|
|
int lambda_reduction(int n, const double *Q, double *Z)
|
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
double *L, *D;
|
|
|
|
int i, j, info;
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (n <= 0) return -1;
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
L = zeros(n, n);
|
|
|
|
D = mat(n, 1);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
for (i = 0; i < n; i++)
|
|
|
|
for (j = 0; j < n; j++)
|
|
|
|
{
|
|
|
|
Z[i + j * n] = i == j ? 1.0 : 0.0;
|
|
|
|
}
|
2017-04-24 22:48:13 +00:00
|
|
|
/* LD factorization */
|
2018-03-03 01:03:39 +00:00
|
|
|
if ((info = LD(n, Q, L, D)))
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
|
|
|
free(L);
|
|
|
|
free(D);
|
|
|
|
return info;
|
|
|
|
}
|
|
|
|
/* lambda reduction */
|
2018-03-03 01:03:39 +00:00
|
|
|
reduction(n, L, D, Z);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
|
|
|
free(L);
|
|
|
|
free(D);
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* mlambda search --------------------------------------------------------------
|
|
|
|
* search by mlambda (ref [2]) for integer least square
|
|
|
|
* args : int n I number of float parameters
|
|
|
|
* int m I number of fixed solutions
|
|
|
|
* double *a I float parameters (n x 1)
|
|
|
|
* 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)
|
|
|
|
*-----------------------------------------------------------------------------*/
|
|
|
|
int lambda_search(int n, int m, const double *a, const double *Q,
|
2018-03-03 01:03:39 +00:00
|
|
|
double *F, double *s)
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
2018-03-03 01:03:39 +00:00
|
|
|
double *L, *D;
|
2017-04-24 22:48:13 +00:00
|
|
|
int info;
|
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
if (n <= 0 || m <= 0) return -1;
|
2017-04-24 22:48:13 +00:00
|
|
|
|
2018-03-03 01:03:39 +00:00
|
|
|
L = zeros(n, n);
|
|
|
|
D = mat(n, 1);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
|
|
|
/* LD factorization */
|
2018-03-03 01:03:39 +00:00
|
|
|
if ((info = LD(n, Q, L, D)))
|
2017-04-24 22:48:13 +00:00
|
|
|
{
|
|
|
|
free(L);
|
|
|
|
free(D);
|
|
|
|
return info;
|
|
|
|
}
|
|
|
|
/* mlambda search */
|
2018-03-03 01:03:39 +00:00
|
|
|
info = search(n, m, L, D, a, F, s);
|
2017-04-24 22:48:13 +00:00
|
|
|
|
|
|
|
free(L);
|
|
|
|
free(D);
|
|
|
|
return info;
|
|
|
|
}
|