写在前面：

这篇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. float corr_baro = 0.0f;     // D
  5. float corr_gps[3][2] = {
  6. { 0.0f, 0.0f },     // N (pos, vel)
  7. { 0.0f, 0.0f },     // E (pos, vel)
  8. { 0.0f, 0.0f },     // D (pos, vel)
  9. };
  10. float corr_vision[3][2] = {
  11. { 0.0f, 0.0f },     // N (pos, vel)
  12. { 0.0f, 0.0f },     // E (pos, vel)
  13. { 0.0f, 0.0f },     // D (pos, vel)
  14. };
  15. float corr_mocap[3][1] = {
  16. { 0.0f },       // N (pos)
  17. { 0.0f },       // E (pos)
  18. { 0.0f },       // D (pos)
  19. };
  20. float corr_lidar = 0.0f;//据说是超声波
  21. float corr_flow[] = { 0.0f, 0.0f }; // N E
  22. bool gps_valid = false;         // GPS is valid
  23. bool lidar_valid = false;       // lidar is valid
  24. bool flow_valid = false;        // flow is valid
  25. bool flow_accurate = false;     // flow should be accurate (this flag not updated if flow_valid == false)
  26. bool vision_valid = false;      // vision is valid
  27. 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. float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
  8. float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
  9. float w_z_gps_p = params.w_z_gps_p * w_gps_z;
  10. float w_z_gps_v = params.w_z_gps_v * w_gps_z;
  11. float w_xy_vision_p = params.w_xy_vision_p;
  12. float w_xy_vision_v = params.w_xy_vision_v;
  13. float w_z_vision_p = params.w_z_vision_p;
  14. float w_mocap_p = params.w_mocap_p;
  15. /* reduce GPS weight if optical flow is good */
  16. if (use_flow && flow_accurate) {
  17. w_xy_gps_p *= params.w_gps_flow;
  18. w_xy_gps_v *= params.w_gps_flow;
  19. }
  20. /* baro offset correction */
  21. if (use_gps_z) {
  22. float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
  23. baro_offset += offs_corr;
  24. corr_baro += offs_corr;
  25. }
  26. /* accelerometer bias correction for GPS (use buffered rotation matrix) */
  27. 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. if (use_gps_z) {
  8. accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
  9. accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
  10. }
  11. /* transform error vector from NED frame to body frame */
  12. for (int i = 0; i < 3; i++) {
  13. float c = 0.0f;
  14. for (int j = 0; j < 3; j++) {
  15. c += R_gps[j][i] * accel_bias_corr[j];
  16. }
  17. if (PX4_ISFINITE(c)) {
  18. acc_bias[i] += c * params.w_acc_bias * dt;
  19. }
  20. }
  21. /* accelerometer bias correction for VISION (use buffered rotation matrix) */
  22. accel_bias_corr[0] = 0.0f;
  23. accel_bias_corr[1] = 0.0f;
  24. accel_bias_corr[2] = 0.0f;
  25. if (use_vision_xy) {
  26. accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
  27. accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
  28. accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
  29. accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
  30. }
  31. if (use_vision_z) {
  32. accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
  33. }
  34. /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
  35. accel_bias_corr[0] = 0.0f;
  36. accel_bias_corr[1] = 0.0f;
  37. accel_bias_corr[2] = 0.0f;
  38. if (use_mocap) {
  39. accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
  40. accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
  41. accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
  42. }
  43. /* transform error vector from NED frame to body frame */
  44. for (int i = 0; i < 3; i++) {
  45. float c = 0.0f;
  46. for (int j = 0; j < 3; j++) {
  47. c += PX4_R(att.R, j, i) * accel_bias_corr[j];
  48. }
  49. if (PX4_ISFINITE(c)) {
  50. acc_bias[i] += c * params.w_acc_bias * dt;
  51. }
  52. }
  53. /* accelerometer bias correction for flow and baro (assume that there is no delay) */
  54. accel_bias_corr[0] = 0.0f;
  55. accel_bias_corr[1] = 0.0f;
  56. accel_bias_corr[2] = 0.0f;
  57. if (use_flow) {
  58. accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
  59. accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
  60. }
  61. if (use_lidar) {
  62. accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;
  63. } else {
  64. accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
  65. }
  66. /* transform error vector from NED frame to body frame */
  67. for (int i = 0; i < 3; i++) {
  68. float c = 0.0f;
  69. for (int j = 0; j < 3; j++) {
  70. c += PX4_R(att.R, j, i) * accel_bias_corr[j];
  71. }
  72. if (PX4_ISFINITE(c)) {
  73. acc_bias[i] += c * params.w_acc_bias * dt;
  74. }
  75. }

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

[cpp] view plain copy
  1. /* sensor combined */
  2. orb_check(sensor_combined_sub, &updated);
  3. if (updated) {
  4. orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
  5. if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
  6. if (att.R_valid) {
  7. /* correct accel bias */
  8. sensor.accelerometer_m_s2[0] -= acc_bias[0];
  9. sensor.accelerometer_m_s2[1] -= acc_bias[1];
  10. sensor.accelerometer_m_s2[2] -= acc_bias[2];
  11. /* transform acceleration vector from body frame to NED frame */
  12. for (int i = 0; i < 3; i++) {
  13. acc[i] = 0.0f;
  14. for (int j = 0; j < 3; j++) {
  15. acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
  16. }
  17. }
  18. acc[2] += CONSTANTS_ONE_G;
  19. } else {
  20. memset(acc, 0, sizeof(acc));
  21. }
  22. accel_timestamp = sensor.accelerometer_timestamp[0];
  23. accel_updates++;
  24. }

这里得到修正后的加速度，之后用此加速度进行一次、二次积分得到预计速度和位置(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. if (i == 0) {
  7. x[1] += w * ewdt;
  8. }
  9. }
  10. }

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. if (local_pos.dist_bottom_valid) {
  17. local_pos.dist_bottom = dist_ground;
  18. local_pos.dist_bottom_rate = - z_est[1];
  19. }
  20. local_pos.timestamp = t;
  21. orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
  22. if (local_pos.xy_global && local_pos.z_global) {
  23. /* publish global position */
  24. global_pos.timestamp = t;
  25. global_pos.time_utc_usec = gps.time_utc_usec;
  26. double est_lat, est_lon;
  27. map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
  28. global_pos.lat = est_lat;
  29. global_pos.lon = est_lon;
  30. global_pos.alt = local_pos.ref_alt - local_pos.z;
  31. global_pos.vel_n = local_pos.vx;
  32. global_pos.vel_e = local_pos.vy;
  33. global_pos.vel_d = local_pos.vz;
  34. global_pos.yaw = local_pos.yaw;
  35. global_pos.eph = eph;
  36. global_pos.epv = epv;
  37. if (terrain_estimator.is_valid()) {
  38. global_pos.terrain_alt = global_pos.alt - terrain_estimator.get_distance_to_ground();
  39. global_pos.terrain_alt_valid = true;
  40. } else {
  41. global_pos.terrain_alt_valid = false;
  42. }
  43. global_pos.pressure_alt = sensor.baro_alt_meter[0];
  44. if (vehicle_global_position_pub == NULL) {
  45. vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
  46. } else {
  47. orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
  48. }
  49. }

常用传感器

气压+加速度=高度(此部分摘自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

pixhawk position_estimator_inav.cpp思路整理及数据流相关推荐

  1. WAF绕过思路整理(挺全)

    转载至:https://harmoc.com/secnote/waf%E7%BB%95%E8%BF%87%E6%80%9D%E8%B7%AF%E6%95%B4%E7%90%86.html WAF绕过思 ...

  2. 张铁柱-前端实现《低代码可视化编辑器》(一)思路整理 React-dnd+Ts

    张铁柱-前端实现<低代码可视化编辑器>(一)思路整理 React-dnd+Ts 先上效果: 拖拽生成页面+调整顺序 最近,接到任务做一个低代码编辑器,于是着手整理一下思路,调研一下实现方式 ...

  3. GAN框架研究与思路整理

      本文内容将着重分析当前GAN原理及其应用场景,比对其相对于传统深度学习方法在图像生成等方面的区别 一.原始GAN原理           原始GAN论文中的思想为生成模型与判别模型间的零和博弈,通 ...

  4. VL53L0X 底层思路整理（1）

    传感器资料思路整理 世界上最小的飞行时间测距和姿态探测传感器 特点: •完全集成的微型模块 – 940nm激光VCSEL – VCSEL驱动程序 –带有高级嵌入式测距传感器的微控制器 – 4.4 x ...

  5. C#FFmpeg视频采集与推送RTMP服务器代码思路整理

    C#视频采集与推送RTMP服务器代码思路整理:在看过FFmpeg后是否认为写C#的视频流采集和推送还是一头雾水啊?深有此感.领导是C#的高手,说可以通过C或C++的代码直接复制粘贴到C#工程然后进行适 ...

  6. 多智能体强化学习思路整理

    多智能体强化学习算法思路整理 目录 摘要 背景和意义 研究背景 强化学习 多智能体强化学习与博弈论基础 研究意义 问题与挑战 问题分类 问题分析 环境的不稳定性与可扩展性的平衡 部分可观测的马尔可夫决 ...

  7. 前端实现登录、登出、请求数据的一些思路整理

    前端实现登录.登出.请求数据的一些思路整理(基于React.JWT技术) 登录.登出和数据请求是两种不同的数据交互方式,是互相独立的. 登录.登出基于 JWT(JSON WEB TOKEN) 技术,通 ...

  8. 2018年美国大学生数学建模竞赛原题、翻译及思路整理

    参加了2018年的美赛,整理了一些参考资料,需要的话就拿去吧. 具体内容是:2018年美国大学生数学建模竞赛原题.翻译及思路整理,有兴趣的小伙伴可以看看 链接:https://pan.baidu.co ...

  9. 探索性数据分析的思路整理

    探索性数据分析的思路整理 读取数据 清洗数据,对构建的数据进行整理 探索全局特征, 通过直方图,散点图,聚合函数对数据进行全局的了解 探索数据的分组特征,通过分组操作分析数据集 %matplotlib ...

最新文章

  1. Nignx集成fastDFS后访问Nginx一直在加载中解决
  2. a标签的四种链接状态
  3. 跨考计算机教研室,跨考教研室专家：脱离题海沉浮 做到有效做题_跨考网
  4. 【Oracle】并行等待之PX Deq Credit: need buffer
  5. 计算机密码发明者去世！曾获图灵奖、并启蒙 Unix 诞生！
  6. python set_Python Set联合
  7. C语言冒泡排序三种写法,冒泡排序的三种实现方法
  8. python信号端点检测_语音端点检测（Voice Activity Detection,VAD）
  9. 171221—8421BCD码、进制转换
  10. 用c++两个分数相加并且化简成最简形式，通过类来完成。思想是：求出两个数的最大公约数用来化简和求最大公倍数，具体公式代码中见
  11. Arcgis中地理坐标系转投影坐标系（自定义地理坐标转换）
  12. centos8安装RabbitMQ和erlang
  13. 系统——windows10专业工作站版简单优化
  14. 【Linux】syscall系统调用原理及实现
  15. ArcGIS数据管理
  16. JAVA设计模式之——抽象工厂模式
  17. 北京理工大学ACM冬季培训课程之C++的应用
  18. 一、Android Matrix 矩阵
  19. 电网故障定位与隔离(配网自动化)
  20. JavaWeb 初识网络

热门文章

  1. 使用python读取txt坐标文件生成挖空矿山_探矿批量
  2. Spring MVC如何接收浏览器传递来的请求参数--request--形参--实体类封装
  3. vue-router 按需加载
  4. Python之分享常用的五款动态数据可视化工具
  5. App设计灵感之十二组精美的天气预报App设计案例
  6. iOS之深入解析bitcode的功能与应用
  7. python下常用OpenCV代码
  8. 5.2 部分依赖图 (Partial Dependence Plot, PDP)
  9. 316. Remove Duplicate Letters 去除重复字母
  10. 《每日一题》62. Unique Paths 不同路径