程序员经验分享-hwhale

pixhawk position_estimator_inav.cpp思路整理及数据流

2023-05-16

写在前面：

这篇blog主要参考pixhawk的高度解算算法解读，并且加以扩展，扩展到其他传感器，其实里面处理好多只是记录了流程，至于里面实际物理意义并不是很清楚，也希望大牛能够指导一下。

概述：

整个算法的核心思想是由地理坐标系下的加速度通过积分，来获得速度、位置信息；经过2次修正产生可利用的信息，第一次是利用传感器计算修正系数产生加速度的偏差修正加速度，第二次是利用修正系数修正位置；最后可利用速度经过加速度修正，可利用的位置经过了加速度和位置修正。加速度的修正过程是由机体测量的加速度通过减去偏差，再转换到地理坐标系；位置的修正统一利用inertial_filter_correct()函数。

这里传感器的作用就是计算一个校正系数来对加速度偏移量进行校正。

代码思路

1. 变量初始化。

[cpp] view plain copy
  1. float z_est[2] = { 0.0f, 0.0f }; // z轴的高度、速度  
  2. float acc[] = { 0.0f, 0.0f, 0.0f }; //地理坐标系（NED）的加速度数据  
  3. float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // 机体坐标系下的加速度偏移量  
  4.   
  5. float corr_baro = 0.0f;     // D  
  6. float corr_gps[3][2] = {  
  7.     { 0.0f, 0.0f },     // N (pos, vel)  
  8.     { 0.0f, 0.0f },     // E (pos, vel)  
  9.     { 0.0f, 0.0f },     // D (pos, vel)  
  10. };  
  11. float corr_vision[3][2] = {  
  12.     { 0.0f, 0.0f },     // N (pos, vel)  
  13.     { 0.0f, 0.0f },     // E (pos, vel)  
  14.     { 0.0f, 0.0f },     // D (pos, vel)  
  15. };  
  16. float corr_mocap[3][1] = {  
  17.     { 0.0f },       // N (pos)  
  18.     { 0.0f },       // E (pos)  
  19.     { 0.0f },       // D (pos)  
  20. };  
  21. float corr_lidar = 0.0f;//据说是超声波  
  22. float corr_flow[] = { 0.0f, 0.0f }; // N E  
  23.   
  24. bool gps_valid = false;         // GPS is valid  
  25. bool lidar_valid = false;       // lidar is valid  
  26. bool flow_valid = false;        // flow is valid  
  27. bool flow_accurate = false;     // flow should be accurate (this flag not updated if flow_valid == false)  
  28. bool vision_valid = false;      // vision is valid  
  29. bool mocap_valid = false;       // mocap is valid  

2. 计算气压计高度的零点偏移，主要是取200个数据求平均。

[cpp] view plain copy
  1. baro_offset += sensor.baro_alt_meter;  
  2. baro_offset /= (float) baro_init_cnt;  

3.各传感器计算得带各自的修正系数和权重

[cpp] view plain copy
  1. corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];  
  2. corr_lidar = lidar_offset - dist_ground - z_est[0];  
  3. corr_flow[0] = flow_v[0] - x_est[1]; /* velocity correction */  
  4. corr_flow[1] = flow_v[1] - y_est[1];  
  5. corr_vision[0][0] = vision.x - x_est[0]; /* calculate correction for position */  
  6. corr_vision[1][0] = vision.y - y_est[0];  
  7. corr_vision[2][0] = vision.z - z_est[0];  
  8. corr_vision[0][1] = vision.vx - x_est[1]; /* calculate correction for velocity */  
  9. corr_vision[1][1] = vision.vy - y_est[1];  
  10. corr_vision[2][1] = vision.vz - z_est[1];  
  11. corr_mocap[0][0] = mocap.x - x_est[0]; /* calculate correction for position */  
  12. corr_mocap[1][0] = mocap.y - y_est[0];  
  13. corr_mocap[2][0] = mocap.z - z_est[0];  
  14. corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; /* calculate correction for position */  
  15. corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];  
  16. corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];  
  17. corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];/* calculate correction for velocity */  
  18. corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];  
  19. corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];  
  20. w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);  
  21. w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);  

4.判断是否超时

[cpp] view plain copy
  1. if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout))  
  2. if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout)))  
  3. if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout)))  
  4. if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout)))  
  5. if (lidar_valid && (t > (lidar_time + lidar_timeout)))  

5.判断是用哪一个传感器

[cpp] view plain copy
  1. /* use GPS if it's valid and reference position initialized */  
  2. bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;  
  3. bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;  
  4. /* use VISION if it's valid and has a valid weight parameter */  
  5. bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;  
  6. bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;  
  7. /* use MOCAP if it's valid and has a valid weight parameter */  
  8. bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap  
  9. if (params.disable_mocap) { //disable mocap if fake gps is used  
  10.     use_mocap = false;  
  11. }  
  12. /* use flow if it's valid and (accurate or no GPS available) */  
  13. bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);  
  14. /* use LIDAR if it's valid and lidar altitude estimation is enabled */  
  15. use_lidar = lidar_valid && params.enable_lidar_alt_est;  

6.计算权重

[cpp] view plain copy
  1. float flow_q = flow.quality / 255.0f;  
  2. float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);  
  3. w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);  
  4. if (!flow_accurate) {  
  5.     w_flow *= 0.05f;  
  6. }  
  7.   
  8. float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;  
  9. float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;  
  10. float w_z_gps_p = params.w_z_gps_p * w_gps_z;  
  11. float w_z_gps_v = params.w_z_gps_v * w_gps_z;  
  12.   
  13. float w_xy_vision_p = params.w_xy_vision_p;  
  14. float w_xy_vision_v = params.w_xy_vision_v;  
  15. float w_z_vision_p = params.w_z_vision_p;  
  16.   
  17. float w_mocap_p = params.w_mocap_p;  
  18. /* reduce GPS weight if optical flow is good */  
  19. if (use_flow && flow_accurate) {  
  20.     w_xy_gps_p *= params.w_gps_flow;  
  21.     w_xy_gps_v *= params.w_gps_flow;  
  22. }  
  23. /* baro offset correction */  
  24. if (use_gps_z) {  
  25.     float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;  
  26.     baro_offset += offs_corr;  
  27.     corr_baro += offs_corr;  
  28. }  
  29. /* accelerometer bias correction for GPS (use buffered rotation matrix) */  
  30. float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };  

7.根据使用的传感器计算加速度偏差

[cpp] view plain copy
  1. if (use_gps_xy) {  
  2.     accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;  
  3.     accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;  
  4.     accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;  
  5.     accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;  
  6. }  
  7.   
  8. if (use_gps_z) {  
  9.     accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;  
  10.     accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;  
  11. }  
  12.   
  13. /* transform error vector from NED frame to body frame */  
  14. for (int i = 0; i < 3; i++) {  
  15.     float c = 0.0f;  
  16.   
  17.     for (int j = 0; j < 3; j++) {  
  18.         c += R_gps[j][i] * accel_bias_corr[j];  
  19.     }  
  20.   
  21.     if (PX4_ISFINITE(c)) {  
  22.         acc_bias[i] += c * params.w_acc_bias * dt;  
  23.     }  
  24. }  
  25.   
  26. /* accelerometer bias correction for VISION (use buffered rotation matrix) */  
  27. accel_bias_corr[0] = 0.0f;  
  28. accel_bias_corr[1] = 0.0f;  
  29. accel_bias_corr[2] = 0.0f;  
  30.   
  31. if (use_vision_xy) {  
  32.     accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;  
  33.     accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;  
  34.     accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;  
  35.     accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;  
  36. }  
  37.   
  38. if (use_vision_z) {  
  39.     accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;  
  40. }  
  41.   
  42. /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */  
  43. accel_bias_corr[0] = 0.0f;  
  44. accel_bias_corr[1] = 0.0f;  
  45. accel_bias_corr[2] = 0.0f;  
  46.   
  47. if (use_mocap) {  
  48.     accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;  
  49.     accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;  
  50.     accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;  
  51. }  
  52.   
  53. /* transform error vector from NED frame to body frame */  
  54. for (int i = 0; i < 3; i++) {  
  55.     float c = 0.0f;  
  56.   
  57.     for (int j = 0; j < 3; j++) {  
  58.         c += PX4_R(att.R, j, i) * accel_bias_corr[j];  
  59.     }  
  60.   
  61.     if (PX4_ISFINITE(c)) {  
  62.         acc_bias[i] += c * params.w_acc_bias * dt;  
  63.     }  
  64. }  
  65.   
  66. /* accelerometer bias correction for flow and baro (assume that there is no delay) */  
  67. accel_bias_corr[0] = 0.0f;  
  68. accel_bias_corr[1] = 0.0f;  
  69. accel_bias_corr[2] = 0.0f;  
  70.   
  71. if (use_flow) {  
  72.     accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;  
  73.     accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;  
  74. }  
  75.   
  76. if (use_lidar) {  
  77.     accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;  
  78. } else {  
  79.     accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;  
  80. }  
  81.   
  82. /* transform error vector from NED frame to body frame */  
  83. for (int i = 0; i < 3; i++) {  
  84.     float c = 0.0f;  
  85.   
  86.     for (int j = 0; j < 3; j++) {  
  87.         c += PX4_R(att.R, j, i) * accel_bias_corr[j];  
  88.     }  
  89.   
  90.     if (PX4_ISFINITE(c)) {  
  91.         acc_bias[i] += c * params.w_acc_bias * dt;  
  92.     }  
  93. }  

这里得到的acc_bias[]用于前面程序(500行左右)(2016.08.23加)

[cpp] view plain copy
  1. /* sensor combined */  
  2. orb_check(sensor_combined_sub, &updated);  
  3.   
  4. if (updated) {  
  5.     orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);  
  6.   
  7.     if (sensor.accelerometer_timestamp[0] != accel_timestamp) {  
  8.         if (att.R_valid) {  
  9.             /* correct accel bias */  
  10.             sensor.accelerometer_m_s2[0] -= acc_bias[0];  
  11.             sensor.accelerometer_m_s2[1] -= acc_bias[1];  
  12.             sensor.accelerometer_m_s2[2] -= acc_bias[2];  
  13.   
  14.             /* transform acceleration vector from body frame to NED frame */  
  15.             for (int i = 0; i < 3; i++) {  
  16.                 acc[i] = 0.0f;  
  17.   
  18.                 for (int j = 0; j < 3; j++) {  
  19.                     acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];  
  20.                 }  
  21.             }  
  22.   
  23.             acc[2] += CONSTANTS_ONE_G;  
  24.   
  25.         } else {  
  26.             memset(acc, 0, sizeof(acc));  
  27.         }  
  28.   
  29.         accel_timestamp = sensor.accelerometer_timestamp[0];  
  30.         accel_updates++;  
  31.     }  
这里得到修正后的加速度，之后用此加速度进行一次、二次积分得到预计速度和位置(2016.08.23加)

8.预计位置

[cpp] view plain copy
  1. /* inertial filter prediction for altitude */  
  2. if (can_estimate_xy) {  
  3. {  
  4. inertial_filter_predict(dt, x_est, acc[0]);  
  5. inertial_filter_predict(dt, y_est, acc[1]);  
  6. }  
  7. inertial_filter_predict(dt, z_est, acc[2]);  

函数解析

这里x_est、y_est、z_est通过float x[2]传进来来后，经过函数处理直接传回来给x_est、y_est、z_est(这里和C语法有点不同，但是不这么解释就说不过去了)

[cpp] view plain copy
  1. void inertial_filter_predict(float dt, float x[2], float acc)  
  2. {  
  3.     if (isfinite(dt)) {  
  4.         if (!isfinite(acc)) {  
  5.             acc = 0.0f;  
  6.         }  
  7.         x[0] += x[1] * dt + acc * dt * dt / 2.0f;  
  8.         x[1] += acc * dt;  
  9.     }  
  10. }  

9.修正位置

利用传感器得到的速度和位置修正

[cpp] view plain copy
  1. /* inertial filter correction for altitude */  
  2. if (use_lidar) {  
  3.     inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);  
  4. } else {  
  5.     inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);  
  6. }  
  7. if (use_gps_z) {  
  8.     epv = fminf(epv, gps.epv);  
  9.     inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);  
  10.     inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);  
  11. }  
  12. if (use_vision_z) {  
  13.     epv = fminf(epv, epv_vision);  
  14.     inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);  
  15. }  
  16. if (use_mocap) {  
  17.     epv = fminf(epv, epv_mocap);  
  18.     inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);  
  19. }  
  20. if (can_estimate_xy) {  
  21.     /* inertial filter correction for position */  
  22.     if (use_flow) {  
  23.         eph = fminf(eph, eph_flow);  
  24.         inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);  
  25.         inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);  
  26.     }  
  27.     if (use_gps_xy) {  
  28.         eph = fminf(eph, gps.eph);  
  29.         inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);  
  30.         inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);  
  31.         if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {  
  32.             inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);  
  33.             inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);  
  34.         }  
  35.     }  
  36.     if (use_vision_xy) {  
  37.         eph = fminf(eph, eph_vision);  
  38.         inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);  
  39.         inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);  
  40.         if (w_xy_vision_v > MIN_VALID_W) {  
  41.             inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);  
  42.             inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);  
  43.         }  
  44.     }  
  45.     if (use_mocap) {  
  46.         eph = fminf(eph, eph_mocap);  
  47.         inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);  
  48.         inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);  
  49.     }  
  50. }  
  51. /* run terrain estimator */  
  52. terrain_estimator.predict(dt, &att, &sensor, &lidar);  
[cpp] view plain copy
  1. 函数解析  
  2. e是修正系数；dt周期时间；x[2]是2个float型成员的数组，x[0]是位置，x[1]是速度；  
  3. i表示修正是位置还是速度，0是修正位置，1是修正速度；w是权重系数  
  4. 这里x_est、y_est、z_est通过float x[2]传进来来后，经过函数处理直接传回来给x_est、y_est、z_est(这里和C语法有点不同，但是不这么解释就说不过去了)  
[cpp] view plain copy
  1. void inertial_filter_correct(float e, float dt, float x[2], int i, float w)  
  2. {  
  3.     if (isfinite(e) && isfinite(w) && isfinite(dt)) {  
  4.         float ewdt = e * w * dt;  
  5.         x[i] += ewdt;  
  6.   
  7.         if (i == 0) {  
  8.             x[1] += w * ewdt;  
  9.         }  
  10.     }  
  11. }  

10.发布

[cpp] view plain copy
  1. /* publish local position */  
  2. local_pos.xy_valid = can_estimate_xy;  
  3. local_pos.v_xy_valid = can_estimate_xy;  
  4. local_pos.xy_global = local_pos.xy_valid && use_gps_xy;  
  5. local_pos.z_global = local_pos.z_valid && use_gps_z;  
  6. local_pos.x = x_est[0];  
  7. local_pos.vx = x_est[1];  
  8. local_pos.y = y_est[0];  
  9. local_pos.vy = y_est[1];  
  10. local_pos.z = z_est[0];  
  11. local_pos.vz = z_est[1];  
  12. local_pos.yaw = att.yaw;  
  13. local_pos.dist_bottom_valid = dist_bottom_valid;  
  14. local_pos.eph = eph;  
  15. local_pos.epv = epv;  
  16.   
  17. if (local_pos.dist_bottom_valid) {  
  18.     local_pos.dist_bottom = dist_ground;  
  19.     local_pos.dist_bottom_rate = - z_est[1];  
  20. }  
  21.   
  22. local_pos.timestamp = t;  
  23.   
  24. orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);  
  25.   
  26. if (local_pos.xy_global && local_pos.z_global) {  
  27.     /* publish global position */  
  28.     global_pos.timestamp = t;  
  29.     global_pos.time_utc_usec = gps.time_utc_usec;  
  30.   
  31.     double est_lat, est_lon;  
  32.     map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);  
  33.   
  34.     global_pos.lat = est_lat;  
  35.     global_pos.lon = est_lon;  
  36.     global_pos.alt = local_pos.ref_alt - local_pos.z;  
  37.   
  38.     global_pos.vel_n = local_pos.vx;  
  39.     global_pos.vel_e = local_pos.vy;  
  40.     global_pos.vel_d = local_pos.vz;  
  41.   
  42.     global_pos.yaw = local_pos.yaw;  
  43.   
  44.     global_pos.eph = eph;  
  45.     global_pos.epv = epv;  
  46.   
  47.     if (terrain_estimator.is_valid()) {  
  48.         global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground();  
  49.         global_pos.terrain_alt_valid = true;  
  50.   
  51.     } else {  
  52.         global_pos.terrain_alt_valid = false;  
  53.     }  
  54.   
  55.     global_pos.pressure_alt = sensor.baro_alt_meter[0];  
  56.   
  57.     if (vehicle_global_position_pub == NULL) {  
  58.         vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);  
  59.   
  60.     } else {  
  61.         orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);  
  62.     }  
  63. }  

常用传感器

气压+加速度=高度(此部分摘自http://blog.sina.com.cn/s/blog_8fe4f2f40102wo50.html)

1. 变量初始化。

[cpp] view plain copy
  1. float z_est[2] = { 0.0f, 0.0f }; // z轴的高度、速度  
  2. float acc[] = { 0.0f, 0.0f, 0.0f }; //地理坐标系（NED）的加速度数据  
  3. float acc_bias[] = { 0.0f, 0.0f, 0.0f }; //机体坐标系下的加速度偏移量  
  4. float corr_baro = 0.0f; // 气压计校正系数  

2. 计算气压计高度的零点偏移，主要是取200个数据求平均。

[cpp] view plain copy
  1. baro_offset += sensor.baro_alt_meter;  
  2. baro_offset /= (float) baro_init_cnt;  

3. 将传感器获取的机体加速度数据转换到地理坐标系下。

加速度数据要先去除偏移量；

[cpp] view plain copy
  1. sensor.accelerometer_m_s2[0] -=acc_bias[0];  
  2. sensor.accelerometer_m_s2[1] -= acc_bias[1];  
  3. sensor.accelerometer_m_s2[2] -=acc_bias[2];  

然后转换坐标系；

[cpp] view plain copy
  1. acc[i] += PX4_R(att.R, i, j) *sensor.accelerometer_m_s2[j];  

地理坐标系下的z轴加速度是有重力加速度的，因此补偿上去。

[cpp] view plain copy
  1. acc[2] += CONSTANTS_ONE_G;  

4. 计算气压计的校正系数

corr_baro = baro_offset -sensor.baro_alt_meter - z_est[0];

5. 加速度偏移向量校正

accel_bias_corr[2] -= corr_baro *params.w_z_baro * params.w_z_baro;

6. 将偏移向量转换到机体坐标系

c += PX4_R(att.R, j, i) *accel_bias_corr[j];

acc_bias[i] += c * params.w_acc_bias * dt;

7. 加速度推算高度

inertial_filter_predict(dt, z_est, acc[2]);

8. 气压计校正系数进行校正

inertial_filter_correct(corr_baro, dt,z_est, 0, params.w_z_baro);

光流

[cpp] view plain copy
  1. orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);  
  2. if (fabs(rates_setpoint.pitch) < rate_threshold) {  
  3.     //warnx("[inav] test ohne comp");  
  4.     flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)  
  5. }  
  6. else {  
  7.     //warnx("[inav] test mit comp");  
  8.     //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)  
  9.     flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f  
  10.                + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)  
  11. }  
  12. if (fabs(rates_setpoint.roll) < rate_threshold) {  
  13.     flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)  
  14. }  
  15. else {  
  16.     //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)  
  17.     flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f  
  18.                + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)  
  19. }  

得出flow_ang[]


[cpp] view plain copy
  1. float dist_bottom = lidar.current_distance;   
  2. float flow_dist = dist_bottom;   
所以说光流的距离来自lidar，也就是超声波
[cpp] view plain copy
  1. gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];  
  2. gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];  
  3. gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];  
  4. flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;  
  5. flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;  
  6. flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;  
  7. yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);  
  8. yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);  

PX4_R(att.R, i, j)的意思是(att.R[i* 3 + j])

[cpp] view plain copy
  1. if (fabs(rates_setpoint.yaw) < rate_threshold) {  
  2.     flow_m[0] = -flow_ang[0] * flow_dist;  
  3.     flow_m[1] = -flow_ang[1] * flow_dist;  
  4. } else {  
  5.     flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;  
  6.     flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;  
  7. }  

得出flow_m[]光流测量向量

[cpp] view plain copy
  1. flow_m[2] = z_est[1];  
  2. for (int i = 0; i < 2; i++) {  
  3.     for (int j = 0; j < 3; j++) {  
  4.         flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];  
  5.     }  
  6. }  
  7. corr_flow[0] = flow_v[0] - x_est[1];  
  8. corr_flow[1] = flow_v[1] - y_est[1];  

得出corr_flow[]


[cpp] view plain copy
  1. accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;  
  2. accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;  

得出accel_bias_corr[]


PX4_R(att.R, i, j)的意思是(att.R[i* 3 + j])

[cpp] view plain copy
  1. /* transform error vector from NED frame to body frame */  
  2. for (int i = 0; i < 3; i++) {  
  3.     float c = 0.0f;  
  4.     for (int j = 0; j < 3; j++) {  
  5.         c += PX4_R(att.R, j, i) * accel_bias_corr[j];  
  6.     }  
  7.     if (PX4_ISFINITE(c)) {  
  8.         acc_bias[i] += c * params.w_acc_bias * dt;  
  9.     }  
  10. }  

得出acc_bias[]

[cpp] view plain copy
  1. inertial_filter_predict(dt, x_est, acc[0]);  
  2. inertial_filter_predict(dt, y_est, acc[1]);  

得出x_est、y_est

[cpp] view plain copy
  1. inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);  
  2. inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);  

得出修正后的x_est、y_est

GPS

[cpp] view plain copy
  1. orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);  
  2. float alt = gps.alt * 1e-3;  
  3. local_pos.ref_alt = alt + z_est[0];  
  4. est_buf[buf_ptr][0][0] = x_est[0];  
  5. est_buf[buf_ptr][0][1] = x_est[1];  
  6. est_buf[buf_ptr][1][0] = y_est[0];  
  7. est_buf[buf_ptr][1][1] = y_est[1];  
  8. est_buf[buf_ptr][2][0] = z_est[0];  
  9. est_buf[buf_ptr][2][1] = z_est[1];  
  10. map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));  
  11. corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];  
  12. corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];  
  13. corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];  
  14. corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];  
  15. corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];  
  16. corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];  

得出corr_gps[][]

[cpp] view plain copy
  1. bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;  
  2. accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;  
  3. accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;  
  4. accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;  
  5. accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;  
  6. accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;  
  7. accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;  

得出accel_bias_corr[]


[cpp] view plain copy
  1. /* push current rotation matrix to buffer */  
  2. memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));  
  3. /* save rotation matrix at this moment */  
  4. memcpy(R_gps, R_buf[est_i], sizeof(R_gps));  
  5. /* transform error vector from NED frame to body frame */  
  6. for (int i = 0; i < 3; i++) {  
  7.     float c = 0.0f;  
  8.     for (int j = 0; j < 3; j++) {  
  9.         c += R_gps[j][i] * accel_bias_corr[j];  
  10.     }  
  11.     if (PX4_ISFINITE(c)) {  
  12.         acc_bias[i] += c * params.w_acc_bias * dt;  
  13.     }  
  14. }   

得出acc_bias[]

[cpp] view plain copy
  1. // gps[a][b]，a=0则为x方向，a=1则为y方向，a=2则为z方向  
  2. //b=0则为位置，b=1则为速度  
  3. inertial_filter_predict(dt, z_est, acc[2]);  

得出z_est

[cpp] view plain copy
  1. inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);   
  2. inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);  

得出修正后的z_est

[cpp] view plain copy
  1. inertial_filter_predict(dt, x_est, acc[0]);  
  2. inertial_filter_predict(dt, y_est, acc[1]);  
  3. inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);  
  4. inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);  
  5. inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);  
  6. inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);  
[cpp] view plain copy
  1.   


本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

PIXHAWK

position

estimator

inav

cpp

pixhawk position_estimator_inav.cpp思路整理及数据流 的相关文章

  • 如何使用 jQuery 获取垂直滚动站点中当前可见的幻灯片

    这个问题可能听起来很愚蠢 但它却让我伤透了脑筋 我想做一种垂直滚动演示 由不同的幻灯片组成 效果应该与该网站 类似 http www soleilnoir net believein start http www soleilnoir ne

  • 带有位置参数的 Git 别名

    基本上我正在尝试别名 git files 9fa3 执行命令 git diff name status 9fa3 9fa3 但 git 似乎没有将位置参数传递给别名命令 我努力了 alias files git diff name stat

  • 如何在Python中显示列表元素的索引？ [复制]

    这个问题在这里已经有答案了 我有以下代码 hey lol hey water pepsi jam for item in hey print item 我是否在项目之前显示列表中的位置 如下所示 1 lol 2 hey 3 water 4

  • 将元素从容器中分离出来

    我将所有内容包装在具有固定宽度的容器元素中 但我有一个 div 我想 突破 该容器以跨越页面的整个宽度 http dabblet com gist 3207168 http dabblet com gist 3207168 正如您在该示例中

  • 将 div 居中对齐，内容左对齐

    我想要一个以文档为中心的 div div 应该占据所有可以显示内容的空间 并且内容本身应该左对齐 我想要创建的是图像库 行和列居中 当您添加新拇指时 它将向左对齐 Code div div img src http www babybedd

  • 如何使用jQuery查找父元素中某个子元素的位置号

    我有一个 div 包含其他几个包含图像的 div 它看起来像这样 div div class imgHldr img src foo bar png div div class imgHldr img src foo bar png div

  • jquery查找元素的位置

    ul li one li li element li li text li li val li ul 我怎样才能获得职位ul被点击的li 我想这会为你做的 li click function alert this index 请注意 ind

  • javascript中div的随机位置

    我正在尝试使用 javascript 使 Div 随机出现在网页上的任何位置 因此 一个 div 出现然后消失 然后另一个 div 出现在页面上的其他位置然后消失 然后另一个 div 再次出现在页面上的另一个随机位置然后消失 依此类推 我不

  • jquery 对话框打开时窗口向上滚动

    我正在尝试使用 jquery 1 4 和 jquery ui 1 8rc3 custom js 打开模态 jquery 对话框 在所有浏览器中 对话框打开都没有问题 但在 IE 7 和 6 中 对话框打开后 窗口会自行滚动到底部 我尝试将窗

  • 文本环绕绝对定位的 div

    我知道有一些关于类似主题的问题 但它们主要涉及浮动 div 图像 我需要将图像 和 div 绝对定位 向右 但我只想让文本围绕它流动 如果我浮动 div 但我无法将其放置在我想要的位置 它会起作用 因为文本只是在图片后面流动 div cla

  • 是否可以在同一个 html 页面中多次使用相对位置？

    我在主页上使用 相对位置 和 绝对位置 我有一个使用上述母版页的页面 并且我尝试在此页面中再次对其他 2 个元素使用 相对位置 和 绝对位置 但该页面中下面的元素 绝对位置 是不是根据其上方的元素放置的 相对位置 而是指母版页中元素的 相对

  • 表单位置位于屏幕右下角

    我正在使用 c WinForm 开发 sman 通知应用程序 我想将主窗体放置在屏幕工作区域的右下角 在多个屏幕的情况下 有没有办法找到最右边的屏幕来放置应用程序 或者至少记住上次使用的屏幕并将表单放置在其右下角 我目前没有多个显示器可供检

  • Android：如何获取RecyclerView当前的X偏移量？

    我正在使用一个Scrollview对于无限的 时间选择器轮播 发现这不是最好的方法 最后一个问题 https stackoverflow com questions 26341587 horizontalscrollview customa

  • android跨度点击事件

    好的 这些都是我的问题 我需要使用正则表达式来过滤掉除字母之外的所有内容 然后我需要将找到的单词包含在 word 标签中 有了这个 str str replaceAll pattern 0 现在我正在过滤所有正确的元素 标点符号 数字等 但

  • 有哪些 CSS 属性可以让元素脱离正常流程？

    有哪些 CSS 属性可以让元素脱离正常流程 这些属性可以是 float position absolute 等 这个问题涉及正常流程的所有可能的改变 只有以下属性会影响任何给定元素的正常流程 float right left positio

  • 如何在 OpenOffice BASIC 宏中通过鼠标单击获取文档坐标

    背景 我想在我用鼠标单击或悬停的位置 使用按键激活时 粘贴 如 CTRL V 任何内容 最好是图像 形状 我不知道该怎么做获取我单击的文档 X Y 上的位置 Apache OpenOffice SDraw Document OpenOffi

  • 如何在 CSS 中正确定位和缩放这些元素？

    我已经能够使用 html 和 css 正确定位和缩放网页中的一些元素 但是由于定位规则 我陷入了如何使用另外两个元素继续此操作的困境 图片中的 V 形图标必须位于标题为 向下滚动 的最后一段下方 我也希望它能够随屏幕尺寸缩放 正如我已经成功

  • 我可以覆盖父元素的 z-index 继承吗？

    使用绝对位置时 有什么方法可以覆盖父元素的 z index 继承 我希望 2222 div 位于 0000 div 之上 div style background color green OOOO div div style backgro

  • CSS - div 与父 div 底部对齐（内联块）

    我知道这个 html 很草率 有一些不必要的额外 div 但无论如何 我无法理解为什么 ID 为 info box right 的 div 与父 div 的底部对齐 您可以看到 文本 与下面的 jsfiddle 示例的底部 有什么想法可以让

  • 组合框下拉位置

    我有一个最大化的表单 其中包含 500px 的组合框控件 停靠在右上角 Width 尝试打开组合框后 列表的一半超出了屏幕 如何强制列表显示在表单中 棘手的问题 我找不到解决这个问题的好办法 只是一个解决方法 添加一个新类并粘贴如下所示的代

随机推荐

  • 【安博．牛耳】嵌入式培训介绍

    培训简介 安博教育集团联手中南地区最大的IT人才输出机构 牛耳软件教育 xff0c 在湖南地区首开专业嵌入式开发工程师培训 嵌入式开发工程师专业培训课程 xff0c 由安博联合各知名企业合作伙伴的精英专家 一线项目总监 经理 优秀技术人员共

  • 最受推荐的10本C/C ++书籍

    链接 xff1a https hackr io blog 10 best c cpp books C和C 43 43 是世界上最流行的编程语言之二 C 43 43 是C语言的扩展 xff0c 这两门语言的潜力都是不可估量的 xff0c 这就

  • Leetcode: Decode ways

    A message containing letters from A Z is being encoded to numbers using the following mapping 39 A 39 gt 1 39 B 39 gt 2

  • 调剂【非全日制】之前必看！关于非全就业情况的一点感想

    原文作者 秦时明月123fly 17年考入帝都的一所985级别的学校非全研究生 xff0c 学制是两年 xff0c 专业是土木口的工程管理 因为是辞职读的非全 xff0c 所以目前也在参加秋招 学土木的都知道 xff0c 要么去施工单位 x

  • 360分！这是某985计算机非全分数线！

    前几天 xff0c 武汉大学公布了计算机相关专业的分数线 xff1a 计算机学院 计算机科学与技术 xff08 学硕 xff09 xff1a 总分 xff1a 380 政治 xff1a 50 外语 xff1a 50 专业课1 xff1a 8

  • 计算机非全日制，究竟值得读吗？

    非全值得读吗 xff1f 作者 xff1a 四川大学 图像所 非全日制 研究生 文章为原创 非全值得读吗 xff1f 我想有这个疑问的大都是应届生 xff0c 而不是往届生或者已经工作的人 xff0c 特别是工作后工资相对较高 xff0c

  • 【非全研究生】到底哪些公司接受？

    很多同学都知道 xff0c 虽然非全在2017年改革了 xff0c 考试难度和毕业难度都加大了 xff0c 但是社会对于非全日制研究生的认可程度依然是个未知数 前几天小编在微博看到一位同学发的 统招非全日制研究生企业认可情况汇总表 xff0

  • 非全凉凉！武汉大学不允许18级19级非全日制硕士研究生参加校招？

    转载于知乎 链接 https www zhihu com question 343870391 作者 xff1a 张铁匠 链接 xff1a https www zhihu com question 343870391 answer 8118

  • 关于博士招生“申请-考核制”，教育部这样说！

    转载于 青塔 近日 xff0c 教育部在官网上就十三届全国人大二次会议 关于改进博士生招生 申请 考核制 的建议 进行答复 答复中称 xff1a 教育部在着力探索建立博士生招生质量第三方评价机制 xff0c 推动招生单位建立健全以自我评价为

  • Keil5.26、Keil5.27、Keil5.30下载地址

    亲测有效 xff0c 速速下载 mdk5 26下载地址 http www keil com fid vquv2wwtdy9j1w9xagw1om5eu9xbkks1e66vd1 files eval mdk526 exe mdk5 27下载

  • EEPROM和flash的区别

    之前对各种存储器一直不太清楚 xff0c 今天总结一下 存储器分为两大类 xff1a ram和rom ram就不讲了 xff0c 今天主要讨论rom rom最初不能编程 xff0c 出厂什么内容就永远什么内容 xff0c 不灵活 后来出现了

  • git merge最简洁用法

    一 开发分支 xff08 dev xff09 上的代码达到上线的标准后 xff0c 要合并到 master 分支 git checkout dev git pull git checkout master git merge dev git

  • 汇编cmp比较指令详解

    刚刚看到了cmp指令 xff0c 一开始有点晕 后来上网找了些资料 xff0c 终于看明白了 xff0c 为了方便初学者 xff0c 我就简单写下我的思路吧 高手绕过 xff0c 谢谢 xff01 cmp compare 指令进行比较两个操

  • vim中复制粘贴

    在vim中要进行复制粘贴 需要使用yy和p指令 但是会发现当我们想讲从vim外面复制的内容粘贴进文本中时 光使用p达不到效果 原因是 在vim中维护者许多的clipboard 而不是只有一个 我们从vim外部 如浏览器 复制的文本所在的cl

  • NP问题真的很难理解

    希望通过这篇文章可以不仅让计算机相关专业的人可以看懂和区分什么是P类问题什么是NP类问题 xff0c 更希望达到的效果是非专业人士比如学文科的朋友也可以有一定程度的理解 有一则程序员界的笑话 xff0c 就是有一哥们去google面试的时候

  • USB转TTL、USB转串口、USB转232的区别

    PO主作为一个没有专业背景的小白 xff0c 在初玩单片机时曾被上面的几个名词所混淆 xff0c 不过后来终于大彻大悟 xff0c 现在把自己的理解写在这里 xff0c 同样准备入门单片机的小白可以看看 xff0c 或许对你有所帮助 首先

  • STM32的时钟系统RCC详细整理

    一 综述 xff1a 1 时钟源 在 STM32 中 xff0c 一共有 5 个时钟源 xff0c 分别是 HSI HSE LSI LSE PLL HSI 是高速内部时钟 xff0c RC 振荡器 xff0c 频率为 8MHz xff1b

  • 第一章 PX4程序编译过程解析

    版权声明 xff1a 本文为博主原创文章 xff0c 未经博主允许不得转载 第一章 PX4程序编译过程解析 PX4是一款软硬件开源的项目 xff0c 目的在于学习和研究 其中也有比较好的编程习惯 xff0c 大家不妨可以学习一下国外牛人的编

  • 第二章 PX4-RCS启动文件解析

    版权声明 xff1a 本文为博主原创文章 xff0c 未经博主允许不得转载 第二章 PX4 RCS启动文件解析 RCS的启动类似于linux的shell文件 xff0c 如果不知道shell文件是什么东西可以理解成是为程序的流程框 xff0

  • pixhawk position_estimator_inav.cpp思路整理及数据流

    写在前面 xff1a 这篇blog主要参考pixhawk的高度解算算法解读 xff0c 并且加以扩展 xff0c 扩展到其他传感器 xff0c 其实里面处理好多只是记录了流程 xff0c 至于里面实际物理意义并不是很清楚 xff0c 也希望

热门标签