Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fast-LIO2's Kalman Filter's modification #5

Open
wants to merge 10 commits into
base: main
Choose a base branch
from
+260 −14,844
Open

Fast-LIO2's Kalman Filter's modification #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
5841a2f
Added Fast-LIO2's Kalman Filter's modification
Huguet57 Dec 25, 2021
68ccdeb
Deleted old version of Fast-LIO
Huguet57 Dec 25, 2021
ba432a9
Deleted VS Code innecessary folder
Huguet57 Dec 25, 2021
516d66f
List of threshold as template
AHuguetUPC Dec 27, 2021
17dab43
Added a bridge for using IKFoM
AHuguetUPC Dec 27, 2021
4a75de9
LIMO-Velo Alpha v0.1
AHuguetUPC Dec 27, 2021
7144fe6
Added Match Class
AHuguetUPC Jan 3, 2022
3df685d
Degeneracy module - First try
AHuguetUPC Jan 22, 2022
fc70a5c
[Merged] Degeneracy as a factor
AHuguetUPC Jan 27, 2022
e53c602
Protect extrinsic eigenvectors from degeneracy
AHuguetUPC Feb 13, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 0 additions & 5 deletions IKFoM_toolkit/esekfom/.vscode/settings.json
View file
Open in desktop

This file was deleted.

147 changes: 89 additions & 58 deletions IKFoM_toolkit/esekfom/esekfom.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,7 @@ struct dyn_share_datastruct
bool valid;
bool converge;
Eigen::Matrix<T, Eigen::Dynamic, 1> z;
Eigen::Matrix<T, Eigen::Dynamic, 1> h;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_v;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> h_x;
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> R;
Expand Down Expand Up @@ -124,7 +125,7 @@ class esekf{
typedef measurement measurementModel(state &, bool &);
typedef measurement measurementModel_share(state &, share_datastruct<state, measurement, measurement_noise_dof> &);
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> measurementModel_dyn(state &, bool &);
typedef Eigen::Matrix<scalar_type, Eigen::Dynamic, 1> measurementModel_dyn_share(state &, dyn_share_datastruct<scalar_type> &);
typedef void measurementModel_dyn_share(state &, dyn_share_datastruct<scalar_type> &);
typedef Eigen::Matrix<scalar_type ,l, n> measurementMatrix1(state &, bool&);
typedef Eigen::Matrix<scalar_type , Eigen::Dynamic, n> measurementMatrix1_dyn(state &, bool&);
typedef Eigen::Matrix<scalar_type ,l, measurement_noise_dof> measurementMatrix2(state &, bool&);
Expand Down Expand Up @@ -233,15 +234,16 @@ class esekf{
//receive system-specific models and their differentions
//for measurement as an Eigen matrix whose dimension is changing.
//calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function (h_dyn_share_in).
void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, scalar_type limit_vector[n])
template <typename Iterator>
void init_dyn_share(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel_dyn_share h_dyn_share_in, int maximum_iteration, Iterator limit_vector)
{
f = f_in;
f_x = f_x_in;
f_w = f_w_in;
h_dyn_share = h_dyn_share_in;

maximum_iter = maximum_iteration;
for(int i=0; i<n; i++)
for(int i=0; i<limit_vector.size(); i++)
{
limit[i] = limit_vector[i];
}
Expand Down Expand Up @@ -378,6 +380,7 @@ class esekf{
F_x1 += f_x_final * dt;
P_ = (F_x1) * P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose();
#endif

}

//iterated error state EKF update for measurement as a manifold.
Expand Down Expand Up @@ -1009,8 +1012,9 @@ class esekf{
for(int i=-1; i<maximum_iter; i++)
{
dyn_share.valid = true;
Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share (x_, dyn_share);
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> z = dyn_share.z;
h_dyn_share (x_, dyn_share);
Matrix<scalar_type, Eigen::Dynamic, 1> z = dyn_share.z;
Matrix<scalar_type, Eigen::Dynamic, 1> h = dyn_share.h;
#ifdef USE_sparse
spMt h_x = dyn_share.h_x.sparseView();
spMt h_v = dyn_share.h_v.sparseView();
Expand Down Expand Up @@ -1611,8 +1615,9 @@ class esekf{
}
}

// Modified version used in Fast-LIO2
//iterated error state EKF update modified for one specific system.
void update_iterated_dyn_share_modified(double R) {
void update_iterated_dyn_share_modified(double R, double D, double &solve_time, bool print_degeneracy=false) {

dyn_share_datastruct<scalar_type> dyn_share;
dyn_share.valid = true;
Expand All @@ -1621,25 +1626,31 @@ class esekf{
state x_propagated = x_;
cov P_propagated = P_;
int dof_Measurement;

Matrix<scalar_type, n, 1> K_h;
Matrix<scalar_type, n, n> K_x;

vectorized_state dx_new = vectorized_state::Zero();
for(int i=-1; i<maximum_iter; i++)
{
dyn_share.valid = true;
Matrix<scalar_type, Eigen::Dynamic, 1> h = h_dyn_share(x_, dyn_share);
#ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView();
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_ = dyn_share.h_x;
#endif
dof_Measurement = h.rows();
vectorized_state dx;
x_.boxminus(dx, x_propagated);
dx_new = dx;

h_dyn_share(x_, dyn_share);

if(! dyn_share.valid)
{
continue;
}

#ifdef USE_sparse
spMt h_x_ = dyn_share.h_x.sparseView();
#else
Eigen::Matrix<scalar_type, Eigen::Dynamic, 12> h_x_ = dyn_share.h_x;
#endif
double solve_start = omp_get_wtime();
dof_Measurement = h_x_.rows();
vectorized_state dx;
x_.boxminus(dx, x_propagated);
dx_new = dx;

P_ = P_propagated;

Expand Down Expand Up @@ -1684,21 +1695,56 @@ class esekf{
P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_;
Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_x;

Eigen::Matrix<scalar_type, 12, 12> HTH;

if(n > dof_Measurement)
{
K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> h_x_cur = Eigen::Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic>::Zero(dof_Measurement, n);
h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_;

Matrix<scalar_type, Eigen::Dynamic, Eigen::Dynamic> K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose()/R + Eigen::Matrix<double, Dynamic, Dynamic>::Identity(dof_Measurement, dof_Measurement)).inverse()/R;
K_h = K_ * dyn_share.h;
K_x = K_ * h_x_cur;
}
else
{
K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose();
#ifdef USE_sparse
spMt A = h_x_.transpose() * h_x_;
cov P_temp = (P_/R).inverse();
P_temp. template block<12, 12>(0, 0) += A;
P_temp = P_temp.inverse();

K_ = P_temp. template block<n, 12>(0, 0) * h_x_.transpose();
K_x = cov::Zero();
K_x. template block<n, 12>(0, 0) = P_inv. template block<n, 12>(0, 0) * HTH;
#else
cov P_temp = (P_/R).inverse();
HTH = h_x_.transpose() * h_x_;
P_temp. template block<12, 12>(0, 0) += HTH;

cov P_inv = P_temp.inverse();
K_h = P_inv. template block<n, 12>(0, 0) * h_x_.transpose() * dyn_share.h;
K_x.setZero();
K_x. template block<n, 12>(0, 0) = P_inv. template block<n, 12>(0, 0) * HTH;
#endif
}
K_x = K_ * h_x_;
Matrix<scalar_type, n, 1> dx_ = K_ * h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;

Matrix<scalar_type, n, 1> dx_ = K_h + (K_x - Matrix<scalar_type, n, n>::Identity()) * dx_new;

// Degeneracy
Eigen::EigenSolver<Eigen::Matrix<scalar_type, 6, 6>> es(HTH. template block<6,6>(0,0));
Eigen::Matrix<scalar_type, 6, 6> VEPs = es.eigenvectors().real(). template block<6,6>(0,0);
Eigen::Matrix<scalar_type, 1, 6> VAPs = es.eigenvalues().real().head(6);
if (VAPs.prod() < 1e-20) VEPs = Eigen::Matrix<scalar_type, 6, 6>::Identity();
Eigen::Matrix<scalar_type, 6, 6> selVEPs = VEPs;
for (int vapi = 0; vapi < 6; ++vapi) if (VAPs(vapi) < D) selVEPs. template block<1,6>(vapi,0) *= 0;
if (print_degeneracy) { for (int vapi = 0; vapi < 6; ++vapi) std::cout << VAPs(vapi) << " "; std::cout << std::endl; }
Matrix<scalar_type, n, 1> dx_no_degenerate_ = dx_;
dx_no_degenerate_.head(6) = VEPs.inverse() * selVEPs * dx_.head(6);

state x_before = x_;
x_.boxplus(dx_);
x_.boxplus(dx_no_degenerate_);
dyn_share.converge = true;
for(int i = 0; i < n ; i++)
{
Expand All @@ -1709,11 +1755,15 @@ class esekf{
}
}
if(dyn_share.converge) t++;

if(!t && i == maximum_iter - 2)
{
dyn_share.converge = true;
}

if(t > 1 || i == maximum_iter - 1)
{
L_ = P_;
std::cout << "iteration time" << t << "," << i << std::endl;
Matrix<scalar_type, 3, 3> res_temp_SO3;
MTK::vect<3, scalar_type> seg_SO3;
for(typename std::vector<std::pair<int, int> >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) {
Expand All @@ -1725,18 +1775,11 @@ class esekf{
for(int i = 0; i < n; i++){
L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i));
}
if(n > dof_Measurement)
{
for(int i = 0; i < dof_Measurement; i++){
K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i));
}
}
else
{
for(int i = 0; i < n; i++){
K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
}

for(int i = 0; i < 12; i++){
K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i));
}

for(int i = 0; i < n; i++){
L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose();
Expand All @@ -1747,7 +1790,7 @@ class esekf{
MTK::vect<2, scalar_type> seg_S2;
for(typename std::vector<std::pair<int, int> >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) {
int idx = (*it).first;

for(int i = 0; i < 2; i++){
seg_S2(i) = dx_(i + idx);
}
Expand All @@ -1760,37 +1803,25 @@ class esekf{
for(int i = 0; i < n; i++){
L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i));
}
if(n > dof_Measurement)
{
for(int i = 0; i < dof_Measurement; i++){
K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i));
}
}
else
{
for(int i = 0; i < n; i++){
K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
}

for(int i = 0; i < 12; i++){
K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i));
}

for(int i = 0; i < n; i++){
L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose();
}
}

if(n > dof_Measurement)
{
P_ = L_ - K_*h_x_ * P_;
}
else
{
P_ = L_ - K_x * P_;
}
P_ = L_ - K_x.template block<n, 12>(0, 0) * P_.template block<12, n>(0, 0);
solve_time += omp_get_wtime() - solve_start;
return;
}
solve_time += omp_get_wtime() - solve_start;
}
}

void change_x(state &input_state)
{
x_ = input_state;
Expand Down
86 changes: 0 additions & 86 deletions Samples/FAST_LIO/CMakeLists.txt
View file
Open in desktop

This file was deleted.

Loading