Skip to content

Commit e5c5b79

Browse files
eastnav2015eastnav2015
committed
version 20190412
1 parent abf4b8c commit e5c5b79

File tree

3 files changed

+99
-60
lines changed

3 files changed

+99
-60
lines changed

‎amsi/apps/gps_imu_loose_ekf.cpp

Lines changed: 51 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -168,12 +168,12 @@ class ekf_loose
168168
bool use_online_imu_cal = 1;
169169
bool online_cal_success = 0;
170170
vector<sensor_msgs::Imu> imu_queue;
171-
int imu_queue_size = 5;
171+
int imu_queue_size = 2;
172172

173173
// EKF related parameters
174174
Eigen::MatrixXd imu_noise_matrix;
175175

176-
Eigen::MatrixXd Q_matrix, G_matrix, sigma_matrix, R_matrix, K_matrix, H_matrix;
176+
Eigen::MatrixXd Q_matrix, G_matrix, sigma_matrix, R_matrix, K_matrix, H_matrix, orientation_matrix, acceleration_rotation;;
177177

178178
VectorXd ekf_state; // state: px, py, pz, vx, vy, vz, bax, bzy, baz
179179
VectorXd ekf_u_t; // ax, ay, az
@@ -191,6 +191,8 @@ class ekf_loose
191191

192192
geometry_msgs::Point32 imu_v, span_v;
193193

194+
double x,y,z,w;
195+
194196

195197
public:
196198
/**
@@ -205,7 +207,7 @@ class ekf_loose
205207
gps_sub = nh.subscribe("/ublox_gps_node/fix", 50, &ekf_loose::ubloxFix_callback,this); // subscribe the result from WLS
206208
span_BP_sub =nh.subscribe("/novatel_data/bestpos", 50, &ekf_loose::span_bp_callback,this);
207209

208-
gps_raw_sub = nh.subscribe("/GNSS_CV", 50, &ekf_loose::GNSS_raw_callback,this);
210+
gps_raw_sub = nh.subscribe("/GNSS_", 50, &ekf_loose::GNSS_raw_callback,this);
209211

210212
heading_sub = nh.subscribe("/novatel_data/inspvax", 500
211213
, &ekf_loose::heading_callback, this); // heading from span-cpt
@@ -266,7 +268,7 @@ void heading_callback(const novatel_msgs::INSPVAXConstPtr& msg) // subscribe the
266268
0, 0, 0, 0, 0, 0, 0.05, 0, 0,
267269
0, 0, 0, 0, 0, 0, 0, 0.05, 0,
268270
0, 0, 0, 0, 0, 0, 0, 0, 0.05;
269-
Q_matrix = Q_matrix * 1;
271+
Q_matrix = Q_matrix * 0.01;
270272

271273
sigma_matrix.resize(9,9);
272274
sigma_matrix << 3.0, 0, 0, 0, 0, 0, 0, 0, 0,
@@ -304,17 +306,17 @@ void heading_callback(const novatel_msgs::INSPVAXConstPtr& msg) // subscribe the
304306
double g_ = 9.8;
305307
// std::cout << " IMU data call back" << input->angular_velocity.x << std::endl;
306308
imu_track = * input;
307-
imu_track.linear_acceleration.x = imu_track.linear_acceleration.x * g_;
308-
imu_track.linear_acceleration.y = imu_track.linear_acceleration.y * g_;
309-
imu_track.linear_acceleration.z = imu_track.linear_acceleration.z * g_;
310-
311-
double imu_roll, imu_pitch, imu_yaw;
312-
tf::Quaternion imu_orientation;
313-
tf::quaternionMsgToTF(input->orientation, imu_orientation);
314-
tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
315-
imu_roll = imu_roll * 180.0 / pi;
316-
imu_pitch = imu_pitch * 180.0 / pi;
317-
imu_yaw = imu_yaw * 180.0 / pi;
309+
// imu_track.linear_acceleration.x = imu_track.linear_acceleration.x * g_;
310+
// imu_track.linear_acceleration.y = imu_track.linear_acceleration.y * g_;
311+
// imu_track.linear_acceleration.z = imu_track.linear_acceleration.z * g_;
312+
313+
// double imu_roll, imu_pitch, imu_yaw;
314+
// tf::Quaternion imu_orientation;
315+
// tf::quaternionMsgToTF(input->orientation, imu_orientation);
316+
// tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
317+
// imu_roll = imu_roll * 180.0 / pi;
318+
// imu_pitch = imu_pitch * 180.0 / pi;
319+
// imu_yaw = imu_yaw * 180.0 / pi;
318320

319321
// std::cout<< " imu_roll -> " <<imu_roll << " imu_pitch-> "<< imu_pitch << " imu_yaw-> " << imu_yaw << std::endl;
320322

@@ -333,6 +335,37 @@ void heading_callback(const novatel_msgs::INSPVAXConstPtr& msg) // subscribe the
333335
// imu_track.linear_acceleration.y = lineAcc_local(1,0);
334336
// imu_track.linear_acceleration.z = lineAcc_local(2,0);
335337
// std::cout<< " imu_track.linear_acceleration.x -> " <<imu_track.linear_acceleration.x << " imu_track.linear_acceleration.y-> " <<imu_track.linear_acceleration.y << std::endl;
338+
339+
340+
341+
/************* transfer the IMU from body to local******************/
342+
// x = imu_track.orientation.x;
343+
// y = imu_track.orientation.y;
344+
// z = imu_track.orientation.z;
345+
// w = imu_track.orientation.w;
346+
347+
// double imu_roll, imu_pitch, imu_yaw;
348+
// tf::Quaternion imu_orientation;
349+
// tf::quaternionMsgToTF(input->orientation, imu_orientation);
350+
// tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
351+
// cout<<"yaw : = " <<imu_yaw*(180/3.14)<<endl;
352+
// // cout<<imu_track.orientation.x<<endl;
353+
// cout<<"imu_track.linear_acceleration.x : = " <<endl<<imu_track.linear_acceleration.x<<endl;
354+
355+
// Eigen::Quaterniond q(w,x,y,z);
356+
// q.normalized();
357+
// Eigen::Matrix3d rotation_matrix;
358+
// rotation_matrix=q.toRotationMatrix();
359+
// orientation_matrix.resize(3,1);
360+
// orientation_matrix<<imu_track.linear_acceleration.x,
361+
// imu_track.linear_acceleration.y,
362+
// imu_track.linear_acceleration.z;
363+
// acceleration_rotation = rotation_matrix.transpose() * orientation_matrix;
364+
365+
// cout<<"acceleration_rotation : = " <<endl<<acceleration_rotation<<endl;
366+
// imu_track.linear_acceleration.x = acceleration_rotation(0,0);
367+
// imu_track.linear_acceleration.y = acceleration_rotation(1,0);
368+
// imu_track.linear_acceleration.z = acceleration_rotation(2,0);
336369

337370

338371
imu_queue.push_back(imu_track);
@@ -609,7 +642,7 @@ void heading_callback(const novatel_msgs::INSPVAXConstPtr& msg) // subscribe the
609642

610643
if(1)
611644
{
612-
bool use_CV_GNSS =1;
645+
bool use_CV_GNSS =0;
613646
double psr_cov = cofactorMatrixCal_single_satellite(GNSS_data.GNSS_Raws[i].elevation, GNSS_data.GNSS_Raws[i].snr);
614647
if(use_CV_GNSS == 0)
615648
{
@@ -666,7 +699,7 @@ void heading_callback(const novatel_msgs::INSPVAXConstPtr& msg) // subscribe the
666699
double odom3_wz = imu_track.angular_velocity.z;
667700
// double odom3_wz = 0.0;
668701

669-
double odom3_vx_std = 10;
702+
double odom3_vx_std = 5;
670703
double odom3_vy_std = 0.1;
671704
double odom3_vz_std = 0.1;
672705

@@ -685,7 +718,7 @@ void heading_callback(const novatel_msgs::INSPVAXConstPtr& msg) // subscribe the
685718
// imu_track.angular_velocity.x, imu_track.angular_velocity.y,imu_track.angular_velocity.z,
686719
// imu_track.linear_acceleration_covariance[3], imu_track.linear_acceleration_covariance[4],imu_track.linear_acceleration_covariance[5],
687720
// imu_track.linear_acceleration_covariance[6],imu_track.linear_acceleration_covariance[7],imu_track.linear_acceleration_covariance[8]); // generateData_gt3 not always span_ecef available at the first epoch
688-
721+
std::cout << std::setprecision(12);
689722
std::cout<<"span_ecef -> "<<span_ecef<<std::endl;
690723
}
691724
Eigen::MatrixXd eWLSSolutionECEF; // 5 unknowns with two clock bias variables

‎amsi/apps/gps_imu_tight_ekf.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -174,7 +174,7 @@ class ekf_tight
174174
bool use_online_imu_cal = 1;
175175
bool online_cal_success = 0;
176176
vector<sensor_msgs::Imu> imu_queue;
177-
int imu_queue_size = 5; // 2000
177+
int imu_queue_size = 2; // 2000
178178

179179
// EKF related parameters
180180
Eigen::MatrixXd imu_noise_matrix;
@@ -201,7 +201,7 @@ class ekf_tight
201201
ekf_tight(bool state)
202202
{
203203
std::cout<<"----------------constructor-----------------"<<std::endl;
204-
imu_sub = nh.subscribe("/imu/data", 50, &ekf_tight::imu_callback,this);
204+
imu_sub = nh.subscribe("/imu_rt", 50, &ekf_tight::imu_callback,this);
205205
span_BP_sub =nh.subscribe("/novatel_data/bestpos", 50, &ekf_tight::span_bp_callback,this);
206206
gps_sub = nh.subscribe("/GNSS_", 50, &ekf_tight::GNSS_raw_callback,this);
207207

@@ -267,7 +267,7 @@ class ekf_tight
267267
0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0,
268268
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3;
269269

270-
Q_matrix = Q_matrix * 10;
270+
Q_matrix = Q_matrix * 0.1;
271271

272272
sigma_matrix.resize(11,11);
273273
sigma_matrix << 3.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

‎amsi/apps/gps_imu_tight_fg.cpp

Lines changed: 45 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -115,9 +115,9 @@ using symbol_shorthand::S; // switch factor
115115
bool enable_IMUPreintegration_factor = 0; // enable imu pre-integration factor
116116
bool enable_Pr_factor = 1; //enable pseudorange factor
117117
bool enbale_CVM_factor = 1; // enable constant velocity factor
118-
bool enbale_SCP_factor = 0; // enable switchable constraint pseudorange factor
118+
bool enbale_SCP_factor = 1; // enable switchable constraint pseudorange factor
119119
bool enable_GNSS_loose_factor = 0; // enbale GNSS loose factor (use WLS solution)
120-
bool enable_IMU_LINEAR_factor = 1; // enable imu linear factor (similar to ekf)
120+
bool enable_IMU_LINEAR_factor = 0; // enable imu linear factor (similar to ekf)
121121

122122
double _vx = 0, _vy = 0, _vz = 0; //
123123
clock_t old_clock = clock();
@@ -162,6 +162,8 @@ nlosExclusion::GNSS_Raw_Array _GNSS_data; // save the GNSS data
162162
Point3 nomXYZ(-2418954.90428,5384679.60663,2407391.40139); // niutoujiao data
163163
// Point3 nomXYZ(-2417729.42895,5384849.60804,2408314.07879); // initial position : kowlontoon data
164164
// Point3 nomXYZ(0, 0, 0); // niutoujiao data
165+
// Point3 nomXYZ(-2419091.11542, 5385376.36368, 2405705.64485); // whomopo
166+
165167
double prediction_pre_t=0;
166168

167169

@@ -367,48 +369,42 @@ double nstamp = 0;
367369

368370

369371
/****in the tightly coupled GNSS/INS integration, transfer imu from body to local frame***/
370-
x = imu_track.orientation.x;
371-
y = imu_track.orientation.y;
372-
z = imu_track.orientation.z;
373-
w = imu_track.orientation.w;
374-
double imu_roll, imu_pitch, imu_yaw;
375-
tf::Quaternion imu_orientation;
376-
tf::quaternionMsgToTF(input->orientation, imu_orientation);
377-
tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
378-
// cout<<"yaw : = " <<imu_yaw*(180/3.14)<<endl;
379-
// cout<<imu_track.orientation.x<<endl;
380-
// cout<<"imu_track.linear_acceleration.x : = " <<endl<<imu_track.linear_acceleration.x<<endl;
381-
Eigen::Quaterniond q(w,x,y,z);
382-
q.normalized();
383-
Eigen::Matrix3d rotation_matrix;
384-
rotation_matrix=q.toRotationMatrix();
372+
373+
Eigen::AngleAxisd rollAngle(0, Eigen::Vector3d::UnitZ());
374+
Eigen::AngleAxisd yawAngle(90, Eigen::Vector3d::UnitY());
375+
Eigen::AngleAxisd pitchAngle(0, Eigen::Vector3d::UnitX());
376+
377+
Eigen::Quaternion<double> q_rotation = rollAngle * yawAngle * pitchAngle;
378+
379+
Eigen::Matrix3d rotationMatrix = q_rotation.matrix();
380+
385381
orientation_matrix.resize(3,1);
386382
orientation_matrix<<imu_track.linear_acceleration.x,
387383
imu_track.linear_acceleration.y,
388384
imu_track.linear_acceleration.z;
389-
acceleration_rotation = rotation_matrix.transpose() * orientation_matrix;
385+
acceleration_rotation = rotationMatrix.transpose() * orientation_matrix;
390386

391387
// cout<<"acceleration_rotation : = " <<endl<<acceleration_rotation<<endl;
392388
imu_track.linear_acceleration.x = acceleration_rotation(0,0);
393389
imu_track.linear_acceleration.y = acceleration_rotation(1,0);
394390
imu_track.linear_acceleration.z = acceleration_rotation(2,0);
395391

396-
/****in the tightly coupled GNSS/INS integration, navigation frame is ECEF***/
397-
if(originllh_span.rows() > 1)
398-
{
399-
double lon = (double)originllh_span(0) * D2R;
400-
double lat = (double)originllh_span(1) * D2R;
392+
// /****in the tightly coupled GNSS/INS integration, navigation frame is ECEF***/
393+
// if(originllh_span.rows() > 1)
394+
// {
395+
// double lon = (double)originllh_span(0) * D2R;
396+
// double lat = (double)originllh_span(1) * D2R;
401397

402398

403-
double e = imu_track.linear_acceleration.x; // acc in enu
404-
double n = imu_track.linear_acceleration.y;
405-
double u = imu_track.linear_acceleration.z;
399+
// double e = imu_track.linear_acceleration.x; // acc in enu
400+
// double n = imu_track.linear_acceleration.y;
401+
// double u = imu_track.linear_acceleration.z;
406402

407403

408-
imu_track.linear_acceleration.x = 0 - sin(lon) * e - cos(lon) * sin(lat) * n + cos(lon) * cos(lat) * u; // transfrom the acc from enu to ecef
409-
imu_track.linear_acceleration.y = 0 + cos(lon) * e - sin(lon) * sin(lat) * n + cos(lat) * sin(lon) * u;
410-
imu_track.linear_acceleration.z = 0 + cos(lat) * n + sin(lat) * u;
411-
}
404+
// imu_track.linear_acceleration.x = 0 - sin(lon) * e - cos(lon) * sin(lat) * n + cos(lon) * cos(lat) * u; // transfrom the acc from enu to ecef
405+
// imu_track.linear_acceleration.y = 0 + cos(lon) * e - sin(lon) * sin(lat) * n + cos(lat) * sin(lon) * u;
406+
// imu_track.linear_acceleration.z = 0 + cos(lat) * n + sin(lat) * u;
407+
// }
412408

413409
imu_up =1;
414410
imu_co++;
@@ -473,6 +469,7 @@ void ubloxFix_callback(const sensor_msgs::NavSatFixConstPtr& fix_msg)
473469

474470
if(ini_navf.latitude == NULL)
475471
{
472+
476473
ini_navf = navfix_;
477474
std::cout<<"ini_navf.header -> "<<ini_navf.header<<std::endl;
478475
originllh.resize(3, 1);
@@ -1180,6 +1177,7 @@ void ubloxFix_callback(const sensor_msgs::NavSatFixConstPtr& fix_msg)
11801177

11811178
if(ini_navf_span.latitude == NULL)
11821179
{
1180+
std::cout << std::setprecision(12);
11831181
ini_navf_span = navfix_;
11841182
std::cout<<"ini_navf_span.header -> "<<ini_navf_span.header<<std::endl;
11851183
originllh_span.resize(3, 1);
@@ -1292,6 +1290,7 @@ int main(int argc, char* argv[])
12921290

12931291
Values initial_values;
12941292
int correction_count = 0;
1293+
int switchable_count = 0;
12951294
initial_values.insert(X(correction_count), prior_pose);
12961295
initial_values.insert(V(correction_count), prior_velocity);
12971296
initial_values.insert(B(correction_count), prior_imu_bias);
@@ -1343,11 +1342,17 @@ int main(int argc, char* argv[])
13431342
// double accel_noise_sigma = 0.03924;
13441343
// double gyro_noise_sigma = 0.0205689024915;
13451344

1346-
double accel_noise_sigma = 0.1624;
1347-
double gyro_noise_sigma = 0.205689024915;
1345+
// double accel_noise_sigma = 0.1624;
1346+
// double gyro_noise_sigma = 0.205689024915;
13481347

1349-
double accel_bias_rw_sigma = 0.1905;
1350-
double gyro_bias_rw_sigma = 0.1454441043;
1348+
// double accel_bias_rw_sigma = 0.1905;
1349+
// double gyro_bias_rw_sigma = 0.1454441043;
1350+
1351+
// We use the sensor specs to build the noise model for the IMU factor.
1352+
double accel_noise_sigma = 0.0003924;
1353+
double gyro_noise_sigma = 0.000205689024915;
1354+
double accel_bias_rw_sigma = 0.004905;
1355+
double gyro_bias_rw_sigma = 0.000001454441043;
13511356

13521357
Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
13531358
Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
@@ -1390,7 +1395,7 @@ int main(int argc, char* argv[])
13901395
// exactly the same, so keeping this for simplicity.
13911396

13921397
// All priors have been set up, now iterate through the data file.
1393-
ros::Rate rate(20); // usually ok when rate(10)
1398+
ros::Rate rate(10); // usually ok when rate(10) 20
13941399
while (ros::ok()) { //ros::ok()
13951400
ros::spinOnce();
13961401
rate.sleep();
@@ -1407,7 +1412,7 @@ int main(int argc, char* argv[])
14071412
imu(3) = imu_track.angular_velocity.x ;
14081413
imu(4) = imu_track.angular_velocity.y ;
14091414
imu(5) = imu_track.angular_velocity.z ;
1410-
1415+
14111416

14121417
// Adding the IMU preintegration.
14131418
double delta_clock_imu = float(clock() - old_imu_clock) / CLOCKS_PER_SEC;
@@ -1528,7 +1533,7 @@ int main(int argc, char* argv[])
15281533
// Point3 between_point(0.5, 0.5, 0.5);
15291534
Point3 between_point(_vx * delta_clock_CVM, _vy * delta_clock_CVM, _vz * delta_clock_CVM);
15301535
Pose3 between_pose(between_rotation, between_point);
1531-
Vector3 between_velocity(0.5, 0.5, 0.5);
1536+
Vector3 between_velocity(0.1, 0.1, 0.1);
15321537
noiseModel::Diagonal::shared_ptr between_pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 5, 5, 5).finished()); // rad,rad,rad,m, m, m
15331538
noiseModel::Diagonal::shared_ptr between_velocity_noise_model = noiseModel::Isotropic::Sigma(3,5); // 0.1 m/s
15341539
noiseModel::Diagonal::shared_ptr between_bias_noise_model = noiseModel::Isotropic::Sigma(6,0.1);
@@ -1540,12 +1545,13 @@ int main(int argc, char* argv[])
15401545
graph->add(BetweenFactor<Vector3>(V(correction_count-1),
15411546
V(correction_count ),
15421547
between_velocity, between_velocity_noise_model));
1548+
imuBias::ConstantBias small_bias(Vector3(0.1, 0.1, 0.1), Vector3(0.1, 0.1, 0.1));
15431549
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
15441550
B(correction_count ),
1545-
zero_bias, bias_noise_model));
1551+
small_bias, bias_noise_model));
15461552

15471553
MotionModel_factor MotionModel_factor_(X(correction_count-1),
1548-
X(correction_count ),V(correction_count - 1), delta_clock_CVM, noiseModel::Isotropic::Sigma(3,5));
1554+
X(correction_count ),V(correction_count-1), delta_clock_CVM, noiseModel::Isotropic::Sigma(3,5));
15491555
graph->add(MotionModel_factor_);
15501556
}
15511557

0 commit comments

Comments
 (0)