mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-06 00:35:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

locationd: modify factor for measurement STDEV in gps update (#22262)

Browse Source 
* modify factor for measurement STDEV in gps update

* add new ref_commit
This commit is contained in:
Vivek Aithal
2021-09-17 22:33:46 -07:00
committed by GitHub
parent dc076baf39
commit 3778592f50
2 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+3 -3
View File
@@ -261,9 +261,9 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(3.0 * log.getVerticalAccuracy(), 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy(), 2)).asDiagonal();
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
this->unix_timestamp_millis = log.getTimestamp();
double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm();
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
c66c5414f85cd98ad2dd7a83989e05990851da74
b575f81975c405417b502479ba15614bc1b3ab9d