寫在前面：
這篇blog主要參考pixhawk的高度解算算法解讀，并且加以擴(kuò)展，擴(kuò)展到其他傳感器，其實(shí)里面處理好多只是記錄了流程，至于里面實(shí)際物理意義并不是很清楚，也希望大牛能夠指導(dǎo)一下。
概述：
整個(gè)算法的核心思想是由地理坐標(biāo)系下的加速度通過積分，來獲得速度、位置信息；經(jīng)過2次修正產(chǎn)生可利用的信息，第一次是利用傳感器計(jì)算修正系數(shù)產(chǎn)生加速度的偏差修正加速度，第二次是利用修正系數(shù)修正位置；最后可利用速度經(jīng)過加速度修正，可利用的位置經(jīng)過了加速度和位置修正。加速度的修正過程是由機(jī)體測(cè)量的加速度通過減去偏差，再轉(zhuǎn)換到地理坐標(biāo)系；位置的修正統(tǒng)一利用inertial_filter_correct()函數(shù)。
這里傳感器的作用就是計(jì)算一個(gè)校正系數(shù)來對(duì)加速度偏移量進(jìn)行校正。
代碼思路
1. 變量初始化。
[cpp] view plain
copy float?z_est[2]?=?{?0.0f,?0.0f?};???float?acc[]?=?{?0.0f,?0.0f,?0.0f?};???float?acc_bias[]?=?{?0.0f,?0.0f,?0.0f?};?????float?corr_baro?=?0.0f;???????float?corr_gps[3][2]?=?{??????{?0.0f,?0.0f?},???????????{?0.0f,?0.0f?},???????????{?0.0f,?0.0f?},???????};??float?corr_vision[3][2]?=?{??????{?0.0f,?0.0f?},???????????{?0.0f,?0.0f?},???????????{?0.0f,?0.0f?},???????};??float?corr_mocap[3][1]?=?{??????{?0.0f?},?????????????{?0.0f?},?????????????{?0.0f?},?????????};??float?corr_lidar?=?0.0f;??float?corr_flow[]?=?{?0.0f,?0.0f?};?????bool?gps_valid?=?false;???????????bool?lidar_valid?=?false;?????????bool?flow_valid?=?false;??????????bool?flow_accurate?=?false;???????bool?vision_valid?=?false;????????bool?mocap_valid?=?false;?????????
2. 計(jì)算氣壓計(jì)高度的零點(diǎn)偏移，主要是取200個(gè)數(shù)據(jù)求平均。
[cpp] view plain
copy baro_offset?+=?sensor.baro_alt_meter;??baro_offset?/=?(float)?baro_init_cnt;??
3.各傳感器計(jì)算得帶各自的修正系數(shù)和權(quán)重
[cpp] view plain
copy corr_baro?=?baro_offset?-?sensor.baro_alt_meter[0]?-?z_est[0];??corr_lidar?=?lidar_offset?-?dist_ground?-?z_est[0];??corr_flow[0]?=?flow_v[0]?-?x_est[1];???corr_flow[1]?=?flow_v[1]?-?y_est[1];??corr_vision[0][0]?=?vision.x?-?x_est[0];???corr_vision[1][0]?=?vision.y?-?y_est[0];??corr_vision[2][0]?=?vision.z?-?z_est[0];??corr_vision[0][1]?=?vision.vx?-?x_est[1];???corr_vision[1][1]?=?vision.vy?-?y_est[1];??corr_vision[2][1]?=?vision.vz?-?z_est[1];??corr_mocap[0][0]?=?mocap.x?-?x_est[0];???corr_mocap[1][0]?=?mocap.y?-?y_est[0];??corr_mocap[2][0]?=?mocap.z?-?z_est[0];??corr_gps[0][0]?=?gps_proj[0]?-?est_buf[est_i][0][0];???corr_gps[1][0]?=?gps_proj[1]?-?est_buf[est_i][1][0];??corr_gps[2][0]?=?local_pos.ref_alt?-?alt?-?est_buf[est_i][2][0];??corr_gps[0][1]?=?gps.vel_n_m_s?-?est_buf[est_i][0][1];??corr_gps[1][1]?=?gps.vel_e_m_s?-?est_buf[est_i][1][1];??corr_gps[2][1]?=?gps.vel_d_m_s?-?est_buf[est_i][2][1];??w_gps_xy?=?min_eph_epv?/?fmaxf(min_eph_epv,?gps.eph);??w_gps_z?=?min_eph_epv?/?fmaxf(min_eph_epv,?gps.epv);??
4.判斷是否超時(shí)
[cpp] view plain
copy if?((flow_valid?||?lidar_valid)?&&?t?>?(flow_time?+?flow_topic_timeout))??if?(gps_valid?&&?(t?>?(gps.timestamp_position?+?gps_topic_timeout)))??if?(vision_valid?&&?(t?>?(vision.timestamp_boot?+?vision_topic_timeout)))??if?(mocap_valid?&&?(t?>?(mocap.timestamp_boot?+?mocap_topic_timeout)))??if?(lidar_valid?&&?(t?>?(lidar_time?+?lidar_timeout)))??
5.判斷是用哪一個(gè)傳感器
[cpp] view plain
copy ??bool?use_gps_xy?=?ref_inited?&&?gps_valid?&&?params.w_xy_gps_p?>?MIN_VALID_W;??bool?use_gps_z?=?ref_inited?&&?gps_valid?&&?params.w_z_gps_p?>?MIN_VALID_W;????bool?use_vision_xy?=?vision_valid?&&?params.w_xy_vision_p?>?MIN_VALID_W;??bool?use_vision_z?=?vision_valid?&&?params.w_z_vision_p?>?MIN_VALID_W;????bool?use_mocap?=?mocap_valid?&&?params.w_mocap_p?>?MIN_VALID_W?&&?params.att_ext_hdg_m?==?mocap_heading;???if?(params.disable_mocap)?{???????use_mocap?=?false;??}????bool?use_flow?=?flow_valid?&&?(flow_accurate?||?!use_gps_xy);????use_lidar?=?lidar_valid?&&?params.enable_lidar_alt_est;??
6.計(jì)算權(quán)重
[cpp] view plain
copy float?flow_q?=?flow.quality?/?255.0f;??float?flow_q_weight?=?(flow_q?-?params.flow_q_min)?/?(1.0f?-?params.flow_q_min);??w_flow?=?PX4_R(att.R,?2,?2)?*?flow_q_weight?/?fmaxf(1.0f,?flow_dist);??if?(!flow_accurate)?{??????w_flow?*=?0.05f;??}????float?w_xy_gps_p?=?params.w_xy_gps_p?*?w_gps_xy;??float?w_xy_gps_v?=?params.w_xy_gps_v?*?w_gps_xy;??float?w_z_gps_p?=?params.w_z_gps_p?*?w_gps_z;??float?w_z_gps_v?=?params.w_z_gps_v?*?w_gps_z;????float?w_xy_vision_p?=?params.w_xy_vision_p;??float?w_xy_vision_v?=?params.w_xy_vision_v;??float?w_z_vision_p?=?params.w_z_vision_p;????float?w_mocap_p?=?params.w_mocap_p;????if?(use_flow?&&?flow_accurate)?{??????w_xy_gps_p?*=?params.w_gps_flow;??????w_xy_gps_v?*=?params.w_gps_flow;??}????if?(use_gps_z)?{??????float?offs_corr?=?corr_gps[2][0]?*?w_z_gps_p?*?dt;??????baro_offset?+=?offs_corr;??????corr_baro?+=?offs_corr;??}????float?accel_bias_corr[3]?=?{?0.0f,?0.0f,?0.0f?};??
7.根據(jù)使用的傳感器計(jì)算加速度偏差
[cpp] view plain
copy if?(use_gps_xy)?{??????accel_bias_corr[0]?-=?corr_gps[0][0]?*?w_xy_gps_p?*?w_xy_gps_p;??????accel_bias_corr[0]?-=?corr_gps[0][1]?*?w_xy_gps_v;??????accel_bias_corr[1]?-=?corr_gps[1][0]?*?w_xy_gps_p?*?w_xy_gps_p;??????accel_bias_corr[1]?-=?corr_gps[1][1]?*?w_xy_gps_v;??}????if?(use_gps_z)?{??????accel_bias_corr[2]?-=?corr_gps[2][0]?*?w_z_gps_p?*?w_z_gps_p;??????accel_bias_corr[2]?-=?corr_gps[2][1]?*?w_z_gps_v;??}??????for?(int?i?=?0;?i?<?3;?i++)?{??????float?c?=?0.0f;????????for?(int?j?=?0;?j?<?3;?j++)?{??????????c?+=?R_gps[j][i]?*?accel_bias_corr[j];??????}????????if?(PX4_ISFINITE(c))?{??????????acc_bias[i]?+=?c?*?params.w_acc_bias?*?dt;??????}??}??????accel_bias_corr[0]?=?0.0f;??accel_bias_corr[1]?=?0.0f;??accel_bias_corr[2]?=?0.0f;????if?(use_vision_xy)?{??????accel_bias_corr[0]?-=?corr_vision[0][0]?*?w_xy_vision_p?*?w_xy_vision_p;??????accel_bias_corr[0]?-=?corr_vision[0][1]?*?w_xy_vision_v;??????accel_bias_corr[1]?-=?corr_vision[1][0]?*?w_xy_vision_p?*?w_xy_vision_p;??????accel_bias_corr[1]?-=?corr_vision[1][1]?*?w_xy_vision_v;??}????if?(use_vision_z)?{??????accel_bias_corr[2]?-=?corr_vision[2][0]?*?w_z_vision_p?*?w_z_vision_p;??}??????accel_bias_corr[0]?=?0.0f;??accel_bias_corr[1]?=?0.0f;??accel_bias_corr[2]?=?0.0f;????if?(use_mocap)?{??????accel_bias_corr[0]?-=?corr_mocap[0][0]?*?w_mocap_p?*?w_mocap_p;??????accel_bias_corr[1]?-=?corr_mocap[1][0]?*?w_mocap_p?*?w_mocap_p;??????accel_bias_corr[2]?-=?corr_mocap[2][0]?*?w_mocap_p?*?w_mocap_p;??}??????for?(int?i?=?0;?i?<?3;?i++)?{??????float?c?=?0.0f;????????for?(int?j?=?0;?j?<?3;?j++)?{??????????c?+=?PX4_R(att.R,?j,?i)?*?accel_bias_corr[j];??????}????????if?(PX4_ISFINITE(c))?{??????????acc_bias[i]?+=?c?*?params.w_acc_bias?*?dt;??????}??}??????accel_bias_corr[0]?=?0.0f;??accel_bias_corr[1]?=?0.0f;??accel_bias_corr[2]?=?0.0f;????if?(use_flow)?{??????accel_bias_corr[0]?-=?corr_flow[0]?*?params.w_xy_flow;??????accel_bias_corr[1]?-=?corr_flow[1]?*?params.w_xy_flow;??}????if?(use_lidar)?{??????accel_bias_corr[2]?-=?corr_lidar?*?params.w_z_lidar?*?params.w_z_lidar;??}?else?{??????accel_bias_corr[2]?-=?corr_baro?*?params.w_z_baro?*?params.w_z_baro;??}??????for?(int?i?=?0;?i?<?3;?i++)?{??????float?c?=?0.0f;????????for?(int?j?=?0;?j?<?3;?j++)?{??????????c?+=?PX4_R(att.R,?j,?i)?*?accel_bias_corr[j];??????}????????if?(PX4_ISFINITE(c))?{??????????acc_bias[i]?+=?c?*?params.w_acc_bias?*?dt;??????}??}??
這里得到的acc_bias[]用于前面程序(500行左右)(2016.08.23加)
[cpp] view plain
copy ??orb_check(sensor_combined_sub,?&updated);????if?(updated)?{??????orb_copy(ORB_ID(sensor_combined),?sensor_combined_sub,?&sensor);????????if?(sensor.accelerometer_timestamp[0]?!=?accel_timestamp)?{??????????if?(att.R_valid)?{????????????????????????????sensor.accelerometer_m_s2[0]?-=?acc_bias[0];??????????????sensor.accelerometer_m_s2[1]?-=?acc_bias[1];??????????????sensor.accelerometer_m_s2[2]?-=?acc_bias[2];??????????????????????????????for?(int?i?=?0;?i?<?3;?i++)?{??????????????????acc[i]?=?0.0f;????????????????????for?(int?j?=?0;?j?<?3;?j++)?{??????????????????????acc[i]?+=?PX4_R(att.R,?i,?j)?*?sensor.accelerometer_m_s2[j];??????????????????}??????????????}????????????????acc[2]?+=?CONSTANTS_ONE_G;????????????}?else?{??????????????memset(acc,?0,?sizeof(acc));??????????}????????????accel_timestamp?=?sensor.accelerometer_timestamp[0];??????????accel_updates++;??????}??
這里得到修正后的加速度，之后用此加速度進(jìn)行一次、二次積分得到預(yù)計(jì)速度和位置(2016.08.23加)
8.預(yù)計(jì)位置
[cpp] view plain
copy ??if?(can_estimate_xy)?{??{??inertial_filter_predict(dt,?x_est,?acc[0]);??inertial_filter_predict(dt,?y_est,?acc[1]);??}??inertial_filter_predict(dt,?z_est,?acc[2]);??
函數(shù)解析
這里x_est、y_est、z_est通過float x[2]傳進(jìn)來來后，經(jīng)過函數(shù)處理直接傳回來給x_est、y_est、z_est(這里和C語法有點(diǎn)不同，但是不這么解釋就說不過去了)
[cpp] view plain
copy void?inertial_filter_predict(float?dt,?float?x[2],?float?acc)??{??????if?(isfinite(dt))?{??????????if?(!isfinite(acc))?{??????????????acc?=?0.0f;??????????}??????????x[0]?+=?x[1]?*?dt?+?acc?*?dt?*?dt?/?2.0f;??????????x[1]?+=?acc?*?dt;??????}??}??
9.修正位置
利用傳感器得到的速度和位置修正
[cpp] view plain
copy ??if?(use_lidar)?{??????inertial_filter_correct(corr_lidar,?dt,?z_est,?0,?params.w_z_lidar);??}?else?{??????inertial_filter_correct(corr_baro,?dt,?z_est,?0,?params.w_z_baro);??}??if?(use_gps_z)?{??????epv?=?fminf(epv,?gps.epv);??????inertial_filter_correct(corr_gps[2][0],?dt,?z_est,?0,?w_z_gps_p);??????inertial_filter_correct(corr_gps[2][1],?dt,?z_est,?1,?w_z_gps_v);??}??if?(use_vision_z)?{??????epv?=?fminf(epv,?epv_vision);??????inertial_filter_correct(corr_vision[2][0],?dt,?z_est,?0,?w_z_vision_p);??}??if?(use_mocap)?{??????epv?=?fminf(epv,?epv_mocap);??????inertial_filter_correct(corr_mocap[2][0],?dt,?z_est,?0,?w_mocap_p);??}??if?(can_estimate_xy)?{????????????if?(use_flow)?{??????????eph?=?fminf(eph,?eph_flow);??????????inertial_filter_correct(corr_flow[0],?dt,?x_est,?1,?params.w_xy_flow?*?w_flow);??????????inertial_filter_correct(corr_flow[1],?dt,?y_est,?1,?params.w_xy_flow?*?w_flow);??????}??????if?(use_gps_xy)?{??????????eph?=?fminf(eph,?gps.eph);??????????inertial_filter_correct(corr_gps[0][0],?dt,?x_est,?0,?w_xy_gps_p);??????????inertial_filter_correct(corr_gps[1][0],?dt,?y_est,?0,?w_xy_gps_p);??????????if?(gps.vel_ned_valid?&&?t?<?gps.timestamp_velocity?+?gps_topic_timeout)?{??????????????inertial_filter_correct(corr_gps[0][1],?dt,?x_est,?1,?w_xy_gps_v);??????????????inertial_filter_correct(corr_gps[1][1],?dt,?y_est,?1,?w_xy_gps_v);??????????}??????}??????if?(use_vision_xy)?{??????????eph?=?fminf(eph,?eph_vision);??????????inertial_filter_correct(corr_vision[0][0],?dt,?x_est,?0,?w_xy_vision_p);??????????inertial_filter_correct(corr_vision[1][0],?dt,?y_est,?0,?w_xy_vision_p);??????????if?(w_xy_vision_v?>?MIN_VALID_W)?{??????????????inertial_filter_correct(corr_vision[0][1],?dt,?x_est,?1,?w_xy_vision_v);??????????????inertial_filter_correct(corr_vision[1][1],?dt,?y_est,?1,?w_xy_vision_v);??????????}??????}??????if?(use_mocap)?{??????????eph?=?fminf(eph,?eph_mocap);??????????inertial_filter_correct(corr_mocap[0][0],?dt,?x_est,?0,?w_mocap_p);??????????inertial_filter_correct(corr_mocap[1][0],?dt,?y_est,?0,?w_mocap_p);??????}??}????terrain_estimator.predict(dt,?&att,?&sensor,?&lidar);??
[cpp] view plain
copy 函數(shù)解析??e是修正系數(shù)；dt周期時(shí)間；x[2]是2個(gè)float型成員的數(shù)組，x[0]是位置，x[1]是速度；??i表示修正是位置還是速度，0是修正位置，1是修正速度；w是權(quán)重系數(shù)??這里x_est、y_est、z_est通過float?x[2]傳進(jìn)來來后，經(jīng)過函數(shù)處理直接傳回來給x_est、y_est、z_est(這里和C語法有點(diǎn)不同，但是不這么解釋就說不過去了)??
[cpp] view plain
copy void?inertial_filter_correct(float?e,?float?dt,?float?x[2],?int?i,?float?w)??{??????if?(isfinite(e)?&&?isfinite(w)?&&?isfinite(dt))?{??????????float?ewdt?=?e?*?w?*?dt;??????????x[i]?+=?ewdt;????????????if?(i?==?0)?{??????????????x[1]?+=?w?*?ewdt;??????????}??????}??}??
10.發(fā)布
[cpp] view plain
copy ??local_pos.xy_valid?=?can_estimate_xy;??local_pos.v_xy_valid?=?can_estimate_xy;??local_pos.xy_global?=?local_pos.xy_valid?&&?use_gps_xy;??local_pos.z_global?=?local_pos.z_valid?&&?use_gps_z;??local_pos.x?=?x_est[0];??local_pos.vx?=?x_est[1];??local_pos.y?=?y_est[0];??local_pos.vy?=?y_est[1];??local_pos.z?=?z_est[0];??local_pos.vz?=?z_est[1];??local_pos.yaw?=?att.yaw;??local_pos.dist_bottom_valid?=?dist_bottom_valid;??local_pos.eph?=?eph;??local_pos.epv?=?epv;????if?(local_pos.dist_bottom_valid)?{??????local_pos.dist_bottom?=?dist_ground;??????local_pos.dist_bottom_rate?=?-?z_est[1];??}????local_pos.timestamp?=?t;????orb_publish(ORB_ID(vehicle_local_position),?vehicle_local_position_pub,?&local_pos);????if?(local_pos.xy_global?&&?local_pos.z_global)?{????????????global_pos.timestamp?=?t;??????global_pos.time_utc_usec?=?gps.time_utc_usec;????????double?est_lat,?est_lon;??????map_projection_reproject(&ref,?local_pos.x,?local_pos.y,?&est_lat,?&est_lon);????????global_pos.lat?=?est_lat;??????global_pos.lon?=?est_lon;??????global_pos.alt?=?local_pos.ref_alt?-?local_pos.z;????????global_pos.vel_n?=?local_pos.vx;??????global_pos.vel_e?=?local_pos.vy;??????global_pos.vel_d?=?local_pos.vz;????????global_pos.yaw?=?local_pos.yaw;????????global_pos.eph?=?eph;??????global_pos.epv?=?epv;????????if?(terrain_estimator.is_valid())?{??????????global_pos.terrain_alt?=?global_pos.alt?-?terrain_estimator.get_distance_to_ground();??????????global_pos.terrain_alt_valid?=?true;????????}?else?{??????????global_pos.terrain_alt_valid?=?false;??????}????????global_pos.pressure_alt?=?sensor.baro_alt_meter[0];????????if?(vehicle_global_position_pub?==?NULL)?{??????????vehicle_global_position_pub?=?orb_advertise(ORB_ID(vehicle_global_position),?&global_pos);????????}?else?{??????????orb_publish(ORB_ID(vehicle_global_position),?vehicle_global_position_pub,?&global_pos);??????}??}??
常用傳感器
氣壓+加速度=高度(此部分摘自http://blog.sina.com.cn/s/blog_8fe4f2f40102wo50.html)
1. 變量初始化。
[cpp] view plain
copy float?z_est[2]?=?{?0.0f,?0.0f?};???float?acc[]?=?{?0.0f,?0.0f,?0.0f?};???float?acc_bias[]?=?{?0.0f,?0.0f,?0.0f?};???float?corr_baro?=?0.0f;???
2. 計(jì)算氣壓計(jì)高度的零點(diǎn)偏移，主要是取200個(gè)數(shù)據(jù)求平均。
[cpp] view plain
copy baro_offset?+=?sensor.baro_alt_meter;??baro_offset?/=?(float)?baro_init_cnt;??
3. 將傳感器獲取的機(jī)體加速度數(shù)據(jù)轉(zhuǎn)換到地理坐標(biāo)系下。
加速度數(shù)據(jù)要先去除偏移量；
[cpp] view plain
copy sensor.accelerometer_m_s2[0]?-=acc_bias[0];??sensor.accelerometer_m_s2[1]?-=?acc_bias[1];??sensor.accelerometer_m_s2[2]?-=acc_bias[2];??
然后轉(zhuǎn)換坐標(biāo)系；
[cpp] view plain
copy acc[i]?+=?PX4_R(att.R,?i,?j)?*sensor.accelerometer_m_s2[j];??
地理坐標(biāo)系下的z軸加速度是有重力加速度的，因此補(bǔ)償上去。
[cpp] view plain
copy acc[2]?+=?CONSTANTS_ONE_G;??
4. 計(jì)算氣壓計(jì)的校正系數(shù)
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. 將偏移向量轉(zhuǎn)換到機(jī)體坐標(biāo)系
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. 氣壓計(jì)校正系數(shù)進(jìn)行校正
inertial_filter_correct(corr_baro, dt,z_est, 0, params.w_z_baro);
光流
[cpp] view plain
copy orb_copy(ORB_ID(optical_flow),?optical_flow_sub,?&flow);??if?(fabs(rates_setpoint.pitch)?<?rate_threshold)?{????????????flow_ang[0]?=?(flow.pixel_flow_x_integral?/?(float)flow.integration_timespan?*?1000000.0f)?*?params.flow_k;??}??else?{??????????????????flow_ang[0]?=?((flow.pixel_flow_x_integral?-?flow.gyro_x_rate_integral)?/?(float)flow.integration_timespan?*?1000000.0f?????????????????+?gyro_offset_filtered[0])?*?params.flow_k;??}??if?(fabs(rates_setpoint.roll)?<?rate_threshold)?{??????flow_ang[1]?=?(flow.pixel_flow_y_integral?/?(float)flow.integration_timespan?*?1000000.0f)?*?params.flow_k;??}??else?{????????????flow_ang[1]?=?((flow.pixel_flow_y_integral?-?flow.gyro_y_rate_integral)?/?(float)flow.integration_timespan?*?1000000.0f?????????????????+?gyro_offset_filtered[1])?*?params.flow_k;??}??
得出flow_ang[]
[cpp] view plain
copy float?dist_bottom?=?lidar.current_distance;???float?flow_dist?=?dist_bottom;???
所以說光流的距離來自lidar，也就是超聲波
[cpp] view plain
copy gyro_offset_filtered[0]?=?flow_gyrospeed_filtered[0]?-?att_gyrospeed_filtered[0];??gyro_offset_filtered[1]?=?flow_gyrospeed_filtered[1]?-?att_gyrospeed_filtered[1];??gyro_offset_filtered[2]?=?flow_gyrospeed_filtered[2]?-?att_gyrospeed_filtered[2];??flow_gyrospeed[0]?=?flow.gyro_x_rate_integral?/?(float)flow.integration_timespan?*?1000000.0f;??flow_gyrospeed[1]?=?flow.gyro_y_rate_integral?/?(float)flow.integration_timespan?*?1000000.0f;??flow_gyrospeed[2]?=?flow.gyro_z_rate_integral?/?(float)flow.integration_timespan?*?1000000.0f;??yaw_comp[0]?=?-?params.flow_module_offset_y?*?(flow_gyrospeed[2]?-?gyro_offset_filtered[2]);??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 if?(fabs(rates_setpoint.yaw)?<?rate_threshold)?{??????flow_m[0]?=?-flow_ang[0]?*?flow_dist;??????flow_m[1]?=?-flow_ang[1]?*?flow_dist;??}?else?{??????flow_m[0]?=?-flow_ang[0]?*?flow_dist?-?yaw_comp[0]?*?params.flow_k;??????flow_m[1]?=?-flow_ang[1]?*?flow_dist?-?yaw_comp[1]?*?params.flow_k;??}??
得出flow_m[]光流測(cè)量向量
[cpp] view plain
copy flow_m[2]?=?z_est[1];??for?(int?i?=?0;?i?<?2;?i++)?{??????for?(int?j?=?0;?j?<?3;?j++)?{??????????flow_v[i]?+=?PX4_R(att.R,?i,?j)?*?flow_m[j];??????}??}??corr_flow[0]?=?flow_v[0]?-?x_est[1];??corr_flow[1]?=?flow_v[1]?-?y_est[1];??
得出corr_flow[]
[cpp] view plain
copy accel_bias_corr[0]?-=?corr_flow[0]?*?params.w_xy_flow;??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 ??for?(int?i?=?0;?i?<?3;?i++)?{??????float?c?=?0.0f;??????for?(int?j?=?0;?j?<?3;?j++)?{??????????c?+=?PX4_R(att.R,?j,?i)?*?accel_bias_corr[j];??????}??????if?(PX4_ISFINITE(c))?{??????????acc_bias[i]?+=?c?*?params.w_acc_bias?*?dt;??????}??}??
得出acc_bias[]
[cpp] view plain
copy inertial_filter_predict(dt,?x_est,?acc[0]);??inertial_filter_predict(dt,?y_est,?acc[1]);??
得出x_est、y_est
[cpp] view plain
copy inertial_filter_correct(corr_flow[0],?dt,?x_est,?1,?params.w_xy_flow?*?w_flow);??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 orb_copy(ORB_ID(vehicle_gps_position),?vehicle_gps_position_sub,?&gps);??float?alt?=?gps.alt?*?1e-3;??local_pos.ref_alt?=?alt?+?z_est[0];??est_buf[buf_ptr][0][0]?=?x_est[0];??est_buf[buf_ptr][0][1]?=?x_est[1];??est_buf[buf_ptr][1][0]?=?y_est[0];??est_buf[buf_ptr][1][1]?=?y_est[1];??est_buf[buf_ptr][2][0]?=?z_est[0];??est_buf[buf_ptr][2][1]?=?z_est[1];??map_projection_project(&ref,?lat,?lon,?&(gps_proj[0]),?&(gps_proj[1]));??corr_gps[0][0]?=?gps_proj[0]?-?est_buf[est_i][0][0];??corr_gps[1][0]?=?gps_proj[1]?-?est_buf[est_i][1][0];??corr_gps[2][0]?=?local_pos.ref_alt?-?alt?-?est_buf[est_i][2][0];??corr_gps[0][1]?=?gps.vel_n_m_s?-?est_buf[est_i][0][1];??corr_gps[1][1]?=?gps.vel_e_m_s?-?est_buf[est_i][1][1];??corr_gps[2][1]?=?gps.vel_d_m_s?-?est_buf[est_i][2][1];??
得出corr_gps[][]
[cpp] view plain
copy bool?use_gps_xy?=?ref_inited?&&?gps_valid?&&?params.w_xy_gps_p?>?MIN_VALID_W;??accel_bias_corr[0]?-=?corr_gps[0][0]?*?w_xy_gps_p?*?w_xy_gps_p;??accel_bias_corr[0]?-=?corr_gps[0][1]?*?w_xy_gps_v;??accel_bias_corr[1]?-=?corr_gps[1][0]?*?w_xy_gps_p?*?w_xy_gps_p;??accel_bias_corr[1]?-=?corr_gps[1][1]?*?w_xy_gps_v;??accel_bias_corr[2]?-=?corr_gps[2][0]?*?w_z_gps_p?*?w_z_gps_p;??accel_bias_corr[2]?-=?corr_gps[2][1]?*?w_z_gps_v;??
得出accel_bias_corr[]
[cpp] view plain
copy ??memcpy(R_buf[buf_ptr],?att.R,?sizeof(att.R));????memcpy(R_gps,?R_buf[est_i],?sizeof(R_gps));????for?(int?i?=?0;?i?<?3;?i++)?{??????float?c?=?0.0f;??????for?(int?j?=?0;?j?<?3;?j++)?{??????????c?+=?R_gps[j][i]?*?accel_bias_corr[j];??????}??????if?(PX4_ISFINITE(c))?{??????????acc_bias[i]?+=?c?*?params.w_acc_bias?*?dt;??????}??}???
得出acc_bias[]
[cpp] view plain
copy ????inertial_filter_predict(dt,?z_est,?acc[2]);??
得出z_est
[cpp] view plain
copy inertial_filter_correct(corr_gps[2][0],?dt,?z_est,?0,?w_z_gps_p);???inertial_filter_correct(corr_gps[2][1],?dt,?z_est,?1,?w_z_gps_v);??
得出修正后的z_est
[cpp] view plain
copy inertial_filter_predict(dt,?x_est,?acc[0]);??inertial_filter_predict(dt,?y_est,?acc[1]);??inertial_filter_correct(corr_gps[0][0],?dt,?x_est,?0,?w_xy_gps_p);??inertial_filter_correct(corr_gps[1][0],?dt,?y_est,?0,?w_xy_gps_p);??inertial_filter_correct(corr_gps[0][1],?dt,?x_est,?1,?w_xy_gps_v);??inertial_filter_correct(corr_gps[1][1],?dt,?y_est,?1,?w_xy_gps_v);??
[cpp] view plain
copy ??
總結(jié)
以上是生活随笔為你收集整理的pixhawk position_estimator_inav.cpp思路整理及数据流的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。
如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。
