Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
gnss_utils
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
labrobotica
algorithms
gnss_utils
Commits
cf502f3f
Commit
cf502f3f
authored
3 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
residual and residual_ci
parent
30c544fd
No related branches found
Branches containing commit
No related tags found
2 merge requests
!20
new tag
,
!19
new tag
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/gnss_utils/tdcp.h
+1
-0
1 addition, 0 deletions
include/gnss_utils/tdcp.h
src/tdcp.cpp
+67
-44
67 additions, 44 deletions
src/tdcp.cpp
with
68 additions
and
44 deletions
include/gnss_utils/tdcp.h
+
1
−
0
View file @
cf502f3f
...
@@ -18,6 +18,7 @@ struct TdcpOutput
...
@@ -18,6 +18,7 @@ struct TdcpOutput
Eigen
::
Vector4d
d
=
Eigen
::
Vector4d
::
Zero
();
Eigen
::
Vector4d
d
=
Eigen
::
Vector4d
::
Zero
();
Eigen
::
Matrix4d
cov_d
=
Eigen
::
Matrix4d
::
Zero
();
Eigen
::
Matrix4d
cov_d
=
Eigen
::
Matrix4d
::
Zero
();
double
dt
=
0
;
double
dt
=
0
;
double
residual
=
0
;
double
residual_ci
=
0
;
double
residual_ci
=
0
;
};
};
...
...
This diff is collapsed.
Click to expand it.
src/tdcp.cpp
+
67
−
44
View file @
cf502f3f
...
@@ -288,8 +288,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -288,8 +288,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
TdcpOutput
output
;
TdcpOutput
output
;
Eigen
::
Vector4d
&
d
(
output
.
d
);
Eigen
::
Vector4d
&
d
(
output
.
d
);
Eigen
::
Matrix4d
&
cov_d
(
output
.
cov_d
);
Eigen
::
Matrix4d
&
cov_d
(
output
.
cov_d
);
double
&
residual
(
output
.
residual
);
double
&
residual_ci
(
output
.
residual_ci
);
double
&
residual_ci
(
output
.
residual_ci
);
const
int
n_rows
=
A
.
rows
();
// Initial guess
// Initial guess
Eigen
::
Vector4d
delta_d
(
Eigen
::
Vector4d
::
Zero
());
Eigen
::
Vector4d
delta_d
(
Eigen
::
Vector4d
::
Zero
());
d
=
d_0
;
d
=
d_0
;
...
@@ -297,7 +300,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -297,7 +300,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// initial sagnac correction
// initial sagnac correction
Eigen
::
VectorXd
prev_sagnac_corr
(
s_k
.
cols
());
Eigen
::
VectorXd
prev_sagnac_corr
(
s_k
.
cols
());
if
(
tdcp_params
.
sagnac_correction
==
2
)
if
(
tdcp_params
.
sagnac_correction
==
2
)
for
(
int
row
=
0
;
row
<
A
.
rows
()
;
row
++
)
for
(
int
row
=
0
;
row
<
n_
rows
;
row
++
)
{
{
double
sagnac_corr_r
=
computeSagnacCorrection
(
x_r
,
s_r
.
col
(
row
));
//OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
double
sagnac_corr_r
=
computeSagnacCorrection
(
x_r
,
s_r
.
col
(
row
));
//OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
double
sagnac_corr_k
=
computeSagnacCorrection
(
x_r
+
d_0
.
head
(
3
),
s_k
.
col
(
row
));
//OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
double
sagnac_corr_k
=
computeSagnacCorrection
(
x_r
+
d_0
.
head
(
3
),
s_k
.
col
(
row
));
//OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
...
@@ -308,7 +311,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -308,7 +311,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
for
(
int
j
=
0
;
j
<
tdcp_params
.
max_iterations
;
j
++
)
for
(
int
j
=
0
;
j
<
tdcp_params
.
max_iterations
;
j
++
)
{
{
// fill A and r
// fill A and r
for
(
int
row
=
0
;
row
<
A
.
rows
()
;
row
++
)
for
(
int
row
=
0
;
row
<
n_
rows
;
row
++
)
{
{
// skip discarded rows
// skip discarded rows
if
(
raim_discarded_rows
.
count
(
row
)
!=
0
)
if
(
raim_discarded_rows
.
count
(
row
)
!=
0
)
...
@@ -360,7 +363,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -360,7 +363,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std
::
cout
<<
"Displacement update ="
<<
delta_d
.
head
<
3
>
().
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Displacement update ="
<<
delta_d
.
head
<
3
>
().
transpose
()
<<
std
::
endl
;
printf
(
"Ref distance = %7.3f m
\n
"
,
d_0
.
norm
());
printf
(
"Ref distance = %7.3f m
\n
"
,
d_0
.
norm
());
printf
(
"Computed distance = %7.3f m
\n
"
,
d
.
head
<
3
>
().
norm
());
printf
(
"Computed distance = %7.3f m
\n
"
,
d
.
head
<
3
>
().
norm
());
printf
(
"Tdcp: rows = %lu
\n
"
,
r
.
rows
()
);
printf
(
"Tdcp: rows = %lu
\n
"
,
n_
rows
);
std
::
cout
<<
"Tdcp: r = "
<<
r
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: r = "
<<
r
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: drho_m = "
<<
drho_m
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: drho_m = "
<<
drho_m
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: A =
\n
"
<<
A
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: A =
\n
"
<<
A
<<
std
::
endl
;
...
@@ -374,15 +377,18 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -374,15 +377,18 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
return
output
;
return
output
;
}
}
// residual
_ci
// residual
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
residual
_ci
=
chisq2ci
(
(
r
+
A
*
delta_d
).
squaredNorm
()
/
var_tdcp
,
A
.
rows
())
;
residual
=
(
r
+
A
*
delta_d
).
squaredNorm
()
/
var_tdcp
;
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
residual
_ci
=
chisq2ci
(
(
r
+
A
*
delta_d
).
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
,
1
)
;
residual
=
(
r
+
A
*
delta_d
).
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
;
else
else
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
// residual = (r + A * delta_d).norm();
// residual = (r + A * delta_d).norm();
// residual_ci
residual_ci
=
chisq2ci
(
residual
,
tdcp_params
.
residual_opt
==
0
?
n_rows
:
1
);
#if GNSS_UTILS_TDCP_DEBUG == 1
#if GNSS_UTILS_TDCP_DEBUG == 1
if
(
j
==
0
)
if
(
j
==
0
)
{
{
...
@@ -391,8 +397,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -391,8 +397,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std
::
cout
<<
"Displacement update ="
<<
delta_d
.
head
<
3
>
().
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Displacement update ="
<<
delta_d
.
head
<
3
>
().
transpose
()
<<
std
::
endl
;
printf
(
"Ref distance = %7.3f m
\n
"
,
d_0
.
norm
());
printf
(
"Ref distance = %7.3f m
\n
"
,
d_0
.
norm
());
printf
(
"Computed distance = %7.3f m
\n
"
,
d
.
head
<
3
>
().
norm
());
printf
(
"Computed distance = %7.3f m
\n
"
,
d
.
head
<
3
>
().
norm
());
printf
(
"Tdcp: residual_ci = %13.10f
\n
"
,
residual_ci
);
printf
(
"Tdcp: residual = %13.10f
\n
"
,
residual
);
printf
(
"Tdcp: rows = %lu
\n
"
,
r
.
rows
());
printf
(
"Tdcp: residual_ci = %13.10f
\n
"
,
residual_ci
);
printf
(
"Tdcp: rows = %lu
\n
"
,
n_rows
);
std
::
cout
<<
"Tdcp: r = "
<<
r
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: r = "
<<
r
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: r+A*delta_d = "
<<
(
r
+
A
*
delta_d
).
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: r+A*delta_d = "
<<
(
r
+
A
*
delta_d
).
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: drho_m = "
<<
drho_m
.
transpose
()
<<
std
::
endl
;
std
::
cout
<<
"Tdcp: drho_m = "
<<
drho_m
.
transpose
()
<<
std
::
endl
;
...
@@ -402,15 +409,15 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -402,15 +409,15 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
printf
(
"Tdcp: dT = %8.3f secs
\n
"
,
d
(
3
));
printf
(
"Tdcp: dT = %8.3f secs
\n
"
,
d
(
3
));
std
::
cout
<<
"Check RAIM conditions...
\n
"
;
std
::
cout
<<
"Check RAIM conditions...
\n
"
;
std
::
cout
<<
"
\t
iteration == 0: "
<<
(
j
==
0
?
"OK
\n
"
:
"FAILED
\n
"
);
std
::
cout
<<
"
\t
iteration == 0: "
<<
(
j
==
0
?
"OK
\n
"
:
"FAILED
\n
"
);
std
::
cout
<<
"
\t
A.
rows
()
> tdcp_params.min_common_sats: "
<<
(
A
.
rows
()
>
tdcp_params
.
min_common_sats
?
"OK
\n
"
:
"FAILED
\n
"
);
std
::
cout
<<
"
\t
n_
rows > tdcp_params.min_common_sats: "
<<
(
n_
rows
>
tdcp_params
.
min_common_sats
?
"OK
\n
"
:
"FAILED
\n
"
);
std
::
cout
<<
"
\t
tdcp_params.raim_n > 0: "
<<
(
tdcp_params
.
raim_n
>
0
?
"OK
\n
"
:
"FAILED
\n
"
);
std
::
cout
<<
"
\t
tdcp_params.raim_n > 0: "
<<
(
tdcp_params
.
raim_n
>
0
?
"OK
\n
"
:
"FAILED
\n
"
);
std
::
cout
<<
"
\t
residual_ci >
tdcp_params.max_residual_ci:
"
<<
(
residual_ci
>
tdcp_params
.
max_residual_ci
?
"OK
\n
"
:
"FAILED
\n
"
)
;
std
::
cout
<<
"
\t
tdcp_params.max_residual_ci:
"
<<
tdcp_params
.
max_residual_ci
;
}
}
#endif
#endif
// RAIM ====================================== (after first iteration)
// RAIM ====================================== (after first iteration)
if
(
j
==
0
and
if
(
j
==
0
and
A
.
rows
()
>
tdcp_params
.
min_common_sats
and
n_
rows
>
tdcp_params
.
min_common_sats
and
tdcp_params
.
raim_n
>
0
and
tdcp_params
.
raim_n
>
0
and
residual_ci
>
tdcp_params
.
max_residual_ci
)
residual_ci
>
tdcp_params
.
max_residual_ci
)
{
{
...
@@ -419,22 +426,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -419,22 +426,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
#endif
#endif
int
worst_sat_row
=
-
1
;
int
worst_sat_row
=
-
1
;
double
best_residual
_ci
=
1e12
;
double
best_residual
=
1e12
;
Eigen
::
Vector4d
best_d
;
Eigen
::
Vector4d
best_d
;
// remove up to 'raim_n' observations (if enough satellites and residual
_ci
condition holds)
// remove up to 'raim_n' observations (if enough satellites and residual condition holds)
while
(
raim_discarded_rows
.
size
()
<
tdcp_params
.
raim_n
and
while
(
raim_discarded_rows
.
size
()
<
tdcp_params
.
raim_n
and
A
.
rows
()
-
raim_discarded_rows
.
size
()
>
tdcp_params
.
min_common_sats
and
n_
rows
-
raim_discarded_rows
.
size
()
>
tdcp_params
.
min_common_sats
and
residual_ci
>
tdcp_params
.
max_residual_ci
)
residual_ci
>
tdcp_params
.
max_residual_ci
)
{
{
auto
A_raim
=
A
;
auto
A_raim
=
A
;
auto
r_raim
=
r
;
auto
r_raim
=
r
;
// METHOD A: using normalized RMSE residual
_ci
// METHOD A: using normalized RMSE residual
if
(
tdcp_params
.
residual_opt
==
0
)
if
(
tdcp_params
.
residual_opt
==
0
)
{
{
// solve removing each row
// solve removing each row
for
(
int
row_removed
=
0
;
row_removed
<
A
_
raim
.
rows
()
;
row_removed
++
)
for
(
int
row_removed
=
0
;
row_removed
<
n
_rows
;
row_removed
++
)
{
{
// skip already discarded rows
// skip already discarded rows
if
(
raim_discarded_rows
.
count
(
row_removed
)
!=
0
)
if
(
raim_discarded_rows
.
count
(
row_removed
)
!=
0
)
...
@@ -451,30 +458,34 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -451,30 +458,34 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// no NaN
// no NaN
if
(
!
d_raim
.
array
().
isNaN
().
any
())
if
(
!
d_raim
.
array
().
isNaN
().
any
())
{
{
// residual
_ci
// residual
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
residual
_ci
=
chisq2ci
(
(
r_raim
+
A_raim
*
delta_d_raim
).
squaredNorm
()
/
var_tdcp
,
A_raim
.
rows
()
-
raim_discarded_rows
.
size
())
;
residual
=
(
r_raim
+
A_raim
*
delta_d_raim
).
squaredNorm
()
/
var_tdcp
;
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
residual
_ci
=
chisq2ci
(
(
r_raim
+
A_raim
*
delta_d_raim
).
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
,
1
)
;
residual
=
(
r_raim
+
A_raim
*
delta_d_raim
).
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
;
else
else
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
// residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
// residual = (r_raim + A_raim * delta_d_raim).norm() / (n_rows - 1);
// residual_ci
residual_ci
=
chisq2ci
(
residual
,
tdcp_params
.
residual_opt
==
0
?
n_rows
-
raim_discarded_rows
.
size
()
:
1
);
#if GNSS_UTILS_TDCP_DEBUG == 1
#if GNSS_UTILS_TDCP_DEBUG == 1
std
::
cout
<<
"RAIM excluding row "
<<
row_removed
;
// << std::endl;
std
::
cout
<<
"RAIM excluding row "
<<
row_removed
;
// << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
printf
(
"
\t
residual_ci = %13.10f
\n
"
,
residual_ci
);
printf
(
"
\t
residual = %13.10f
\n
"
,
residual
);
printf
(
"
\t
residual_ci = %13.10f
\n
"
,
residual_ci
);
//std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl;
//std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
//std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
//printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
//printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif
#endif
// store if best residual
_ci
// store if best residual
if
(
residual
_ci
<
best_residual
_ci
)
if
(
residual
<
best_residual
)
{
{
worst_sat_row
=
row_removed
;
worst_sat_row
=
row_removed
;
best_residual
_ci
=
residual
_ci
;
best_residual
=
residual
;
best_d
=
d_raim
;
best_d
=
d_raim
;
}
}
}
}
...
@@ -483,10 +494,10 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -483,10 +494,10 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r_raim
(
row_removed
)
=
r
(
row_removed
);
r_raim
(
row_removed
)
=
r
(
row_removed
);
}
}
}
}
// METHOD B: using max residual
_ci
in Mahalanobis distance
// METHOD B: using max residual in Mahalanobis distance
else
if
(
tdcp_params
.
residual_opt
==
1
)
else
if
(
tdcp_params
.
residual_opt
==
1
)
{
{
// find index of max residual
_ci
// find index of max residual
(
r
+
A
*
delta_d
).
cwiseAbs2
().
maxCoeff
(
&
worst_sat_row
);
(
r
+
A
*
delta_d
).
cwiseAbs2
().
maxCoeff
(
&
worst_sat_row
);
// remove row
// remove row
...
@@ -500,18 +511,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -500,18 +511,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// no NaN
// no NaN
if
(
!
d_raim
.
array
().
isNaN
().
any
())
if
(
!
d_raim
.
array
().
isNaN
().
any
())
{
{
// residual
_ci
// residual
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
residual
_ci
=
chisq2ci
(
(
r_raim
+
A_raim
*
delta_d_raim
).
squaredNorm
()
/
var_tdcp
,
A_raim
.
rows
()
-
raim_discarded_rows
.
size
())
;
residual
=
(
r_raim
+
A_raim
*
delta_d_raim
).
squaredNorm
()
/
var_tdcp
;
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
residual
_ci
=
chisq2ci
(
(
r_raim
+
A_raim
*
delta_d_raim
).
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
,
1
)
;
residual
=
(
r_raim
+
A_raim
*
delta_d_raim
).
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
;
else
else
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
// residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
// residual = (r_raim + A_raim * delta_d_raim).norm() / (n_rows - 1);
// residual_ci
residual_ci
=
chisq2ci
(
residual
,
tdcp_params
.
residual_opt
==
0
?
n_rows
-
raim_discarded_rows
.
size
()
:
1
);
#if GNSS_UTILS_TDCP_DEBUG == 1
#if GNSS_UTILS_TDCP_DEBUG == 1
std
::
cout
<<
"RAIM excluding row "
<<
worst_sat_row
;
// << std::endl;
std
::
cout
<<
"RAIM excluding row "
<<
worst_sat_row
;
// << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
printf
(
"
\t
residual = %13.10f
\n
"
,
residual
);
printf
(
"
\t
residual_ci = %13.10f
\n
"
,
residual_ci
);
printf
(
"
\t
residual_ci = %13.10f
\n
"
,
residual_ci
);
//std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl;
...
@@ -519,8 +534,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -519,8 +534,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
//printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
//printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif
#endif
// store as best residual
_ci
// store as best residual
best_residual
_ci
=
residual
_ci
;
best_residual
=
residual
;
best_d
=
d_raim
;
best_d
=
d_raim
;
}
}
}
}
...
@@ -544,9 +559,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -544,9 +559,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r
(
worst_sat_row
)
=
0
;
r
(
worst_sat_row
)
=
0
;
// Choose the best RAIM solution
// Choose the best RAIM solution
d
=
best_d
;
d
=
best_d
;
cov_d
=
(
A
.
transpose
()
*
A
).
inverse
();
cov_d
=
(
A
.
transpose
()
*
A
).
inverse
();
residual
_ci
=
best_residual
_ci
;
residual
=
best_residual
;
}
}
#if GNSS_UTILS_TDCP_DEBUG == 1
#if GNSS_UTILS_TDCP_DEBUG == 1
...
@@ -558,7 +573,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -558,7 +573,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std
::
cout
<<
"
\t
Initial guess = "
<<
d_0
.
transpose
()
<<
" ("
<<
d_0
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Initial guess = "
<<
d_0
.
transpose
()
<<
" ("
<<
d_0
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Solution = "
<<
d
.
transpose
()
<<
" ("
<<
d
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Solution = "
<<
d
.
transpose
()
<<
" ("
<<
d
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Clock offset = "
<<
d
(
3
)
<<
std
::
endl
;
std
::
cout
<<
"
\t
Clock offset = "
<<
d
(
3
)
<<
std
::
endl
;
std
::
cout
<<
"
\t
Residual = "
<<
best_residual
_ci
<<
std
::
endl
;
std
::
cout
<<
"
\t
Residual = "
<<
best_residual
<<
std
::
endl
;
std
::
cout
<<
"
\t
A =
\n
"
<<
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
A =
\n
"
<<
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
H =
\n
"
<<
A
.
transpose
()
*
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
H =
\n
"
<<
A
.
transpose
()
*
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
cov_d =
\n
"
<<
cov_d
<<
std
::
endl
;
std
::
cout
<<
"
\t
cov_d =
\n
"
<<
cov_d
<<
std
::
endl
;
...
@@ -574,7 +589,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -574,7 +589,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std
::
cout
<<
"
\t
Initial guess = "
<<
d_0
.
transpose
()
<<
" ("
<<
d_0
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Initial guess = "
<<
d_0
.
transpose
()
<<
" ("
<<
d_0
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Solution = "
<<
d
.
transpose
()
<<
" ("
<<
d
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Solution = "
<<
d
.
transpose
()
<<
" ("
<<
d
.
head
<
3
>
().
norm
()
<<
"m)"
<<
std
::
endl
;
std
::
cout
<<
"
\t
Clock offset = "
<<
d
(
3
)
<<
std
::
endl
;
std
::
cout
<<
"
\t
Clock offset = "
<<
d
(
3
)
<<
std
::
endl
;
std
::
cout
<<
"
\t
Residual = "
<<
residual_ci
<<
std
::
endl
;
std
::
cout
<<
"
\t
Residual = "
<<
residual
<<
std
::
endl
;
std
::
cout
<<
"
\t
Residual_ci = "
<<
residual_ci
<<
std
::
endl
;
std
::
cout
<<
"
\t
A =
\n
"
<<
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
A =
\n
"
<<
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
H =
\n
"
<<
A
.
transpose
()
*
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
H =
\n
"
<<
A
.
transpose
()
*
A
<<
std
::
endl
;
std
::
cout
<<
"
\t
cov_d =
\n
"
<<
cov_d
<<
std
::
endl
;
std
::
cout
<<
"
\t
cov_d =
\n
"
<<
cov_d
<<
std
::
endl
;
...
@@ -586,20 +602,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
...
@@ -586,20 +602,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
for
(
int
row
=
0
;
row
<
r
.
size
();
row
++
)
for
(
int
row
=
0
;
row
<
r
.
size
();
row
++
)
if
(
raim_discarded_rows
.
count
(
row
)
==
0
)
if
(
raim_discarded_rows
.
count
(
row
)
==
0
)
r
(
row
)
=
drho_m
(
row
)
-
(
s_k
.
col
(
row
)
-
x_r
-
d
.
head
(
3
)).
norm
()
+
(
s_r
.
col
(
row
)
-
x_r
).
norm
()
-
d
(
3
);
r
(
row
)
=
drho_m
(
row
)
-
(
s_k
.
col
(
row
)
-
x_r
-
d
.
head
(
3
)).
norm
()
+
(
s_r
.
col
(
row
)
-
x_r
).
norm
()
-
d
(
3
);
// residual
_ci
// residual
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
if
(
tdcp_params
.
residual_opt
==
0
)
// all problem
residual
_ci
=
chisq2ci
(
r
.
squaredNorm
()
/
var_tdcp
,
r
.
rows
()
-
raim_discarded_rows
.
size
())
;
residual
=
r
.
squaredNorm
()
/
var_tdcp
;
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
else
if
(
tdcp_params
.
residual_opt
==
1
)
// worst sat
residual
_ci
=
chisq2ci
(
r
.
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
,
1
)
;
residual
=
r
.
cwiseAbs2
().
maxCoeff
()
/
var_tdcp
;
else
else
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
throw
std
::
runtime_error
(
"unknown value for residual_opt"
);
//residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
//residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
// check residual_ci condition
// residual_ci
if
(
tdcp_params
.
validate_residual
and
residual_ci
>
tdcp_params
.
max_residual_ci
)
residual_ci
=
chisq2ci
(
residual
,
tdcp_params
.
residual_opt
==
0
?
n_rows
-
raim_discarded_rows
.
size
()
:
1
);
// check residual condition
if
(
tdcp_params
.
validate_residual
and
residual_ci
>
tdcp_params
.
max_residual_ci
)
{
{
printf
(
"Tdcp: Didn't success. Final residual_ci=%f bigger than max_residual_ci=%f.
\n
"
,
residual_ci
,
tdcp_params
.
max_residual_ci
);
printf
(
"Tdcp: Didn't success. Final residual=%f (CI %f) bigger than max_residual_ci=%f.
\n
"
,
output
.
msg
=
"Residual bigger than max_residual_ci"
;
residual
,
residual_ci
,
tdcp_params
.
max_residual_ci
);
output
.
msg
=
"residual_ci bigger than max_residual_ci"
;
output
.
success
=
false
;
output
.
success
=
false
;
}
}
else
else
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment