fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 19:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

clarify the codes of LPE

Browse Source
This commit is contained in:
stmoon
2018-04-08 11:13:47 -04:00
committed by Kabir Mohammed
parent 3b64325ff8
commit 3ec37aa186
1 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+10 -10
View File
@@ -113,21 +113,21 @@ void BlockLocalPositionEstimator::gpsCorrect()
if (gpsMeasure(y_global) != OK) { return; } if (gpsMeasure(y_global) != OK) { return; }
// gps measurement in local frame // gps measurement in local frame
double lat = y_global(0); double lat = y_global(Y_gps_x);
double lon = y_global(1); double lon = y_global(Y_gps_y);
float alt = y_global(2); float alt = y_global(Y_gps_z);
float px = 0; float px = 0;
float py = 0; float py = 0;
float pz = -(alt - _gpsAltOrigin); float pz = -(alt - _gpsAltOrigin);
map_projection_project(&_map_ref, lat, lon, &px, &py); map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, 6> y; Vector<float, n_y_gps> y;
y.setZero(); y.setZero();
y(0) = px; y(Y_gps_x) = px;
y(1) = py; y(Y_gps_y) = py;
y(2) = pz; y(Y_gps_z) = pz;
y(3) = y_global(3); y(Y_gps_vx) = y_global(Y_gps_vx);
y(4) = y_global(4); y(Y_gps_vy) = y_global(Y_gps_vy);
y(5) = y_global(5); y(Y_gps_vz) = y_global(Y_gps_vz);
// gps measurement matrix, measures position and velocity // gps measurement matrix, measures position and velocity
Matrix<float, n_y_gps, n_x> C; Matrix<float, n_y_gps, n_x> C;