by luoshi006
歡迎交流~ 個人 Gitter 交流平臺，點擊直達：

參考：
1. http://dev.px4.io/advanced-switching_state_estimators.html
2. http://blog.sina.com.cn/s/blog_8fe4f2f40102whmb.html

0、 概述

幾種位置估計

INAV position estimator【組合導航 integrated navigation】
The INAV position estimator is a complementary filter for 3D position and velocity states.

LPE position estimator【Local position estimator】
The LPE position estimator is an extended kalman filter for 3D position and velocity states.

EKF2 attitude, position and wind states estimator
EKF2 is an extended kalman filter estimating attitude, 3D position / velocity and wind states.

位置估計切換

配置 SYS_MC_EST_GROUP：

SYS_MC_EST_GROUPQ EstimatorINAVLPEEKF2

0	1	1	?	?
1	1	?	1	?
2	?	?	?	1

INAV 原理

預測函數：

s+=vt+12at2

v+=at

校正部分：

沒看懂。。。。。
這部分應該是在用 二階低通濾波 ，不過具體過程并沒有推導出來，數學基礎有待提升啊。。。

【有興趣的同學可以從二階低通濾波的復頻域形式進行推導】，也希望有思路的同學不吝賜教~~~

另：

主程序中，在最后也大量使用了這種濾波，對加速度偏差信息進行校正；
不過默認參數 INAV_W_XY_GPS_P=1，INAV_W_XY_GPS_V = 2，在濾波中位置信息權重很詭異；有待研究。

發布信息

orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);

orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);

---

注：
程序中 lidar 對應的發布信息是 distance_sensor，即測距傳感器。
在 msg 中： uint8 type # Type from MAV_DISTANCE_SENSOR enum uint8 MAV_DISTANCE_SENSOR_LASER = 0 //lidar; uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1//超聲; uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 //紅外;

---

1、 文件

文件位置：Firmware/src/modules/position_estimator_inav/

//源碼截取日期：20160826；--position_estimator_inav_main.cpp--position_estimator_inav_params.cpp--position_estimator_inav_params.h--inertial_filter.cpp--inertial_filter.h

2、 inertial_filter.c

/** inertial_filter.c*/#include <math.h> #include "inertial_filter.h"void inertial_filter_predict(float dt, float x[2], float acc) {// x[0] = position; x[1] = velocity;if (isfinite(dt)) {if (!isfinite(acc)) {acc = 0.0f;}x[0] += x[1] * dt + acc * dt * dt / 2.0f;//參考初中物理位移公式；x[1] += acc * dt;//參考初中物理速度公式；} }void inertial_filter_correct(float e, float dt, float x[2], int i, float w) {// e = error when sensor update; x[0] = position; x[1] = velocity; w = weight;if (isfinite(e) && isfinite(w) && isfinite(dt)) {float ewdt = e * w * dt;x[i] += ewdt;//低通濾波;if (i == 0) {//若上面是 位置 校正，則同時進行 速度 校正；x[1] += w * ewdt;}} }

3、 position_estimator_inav_params

/** @file position_estimator_inav_params.c** @author Anton Babushkin <rk3dov@gmail.com>** Parameters for position_estimator_inav*/#include "position_estimator_inav_params.h"/*** Z axis weight for barometer* 氣壓計 z軸 權重（截止頻率）* Weight (cutoff frequency) for barometer altitude measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);/*** Z axis weight for GPS* GPS z軸 權重（截止頻率）* Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);/*** Z velocity weight for GPS* GPS z軸速度 權重（截止頻率）* Weight (cutoff frequency) for GPS altitude velocity measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);/*** Z axis weight for vision* 視覺 z軸 權重（截止頻率）* Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f);/*** Z axis weight for lidar* lidar（雷達） z軸 權重（截止頻率）* Weight (cutoff frequency) for lidar measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f);/*** XY axis weight for GPS position* GPS xy軸位置 權重（截止頻率）* Weight (cutoff frequency) for GPS position measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);/*** XY axis weight for GPS velocity* GPS xy軸速度 權重（截止頻率）* Weight (cutoff frequency) for GPS velocity measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);/*** XY axis weight for vision position* 視覺 xy軸位置 權重（截止頻率）* Weight (cutoff frequency) for vision position measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f);/*** XY axis weight for vision velocity* 視覺 xy軸速度 權重（截止頻率）* Weight (cutoff frequency) for vision velocity measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);/*** Weight for mocap system* 動作捕捉系統 位置 權重* Weight (cutoff frequency) for mocap position measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f);/*** XY axis weight for optical flow* 光流 xy軸 權重* Weight (cutoff frequency) for optical flow (velocity) measurements.** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 0.8f);/*** XY axis weight for resetting velocity* 速度重置 xy軸 權重* When velocity sources lost slowly decrease estimated horizontal velocity with this weight.* 速度信號丟失后，依此權重緩慢減少水平面的速度估計值；* @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);/*** XY axis weight factor for GPS when optical flow available* 啟用光流時 GPS xy軸 權重因子* When optical flow data available, multiply GPS weights (for position and velocity) by this factor.** @min 0.0* @max 1.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);/*** Accelerometer bias estimation weight* 加速度計 偏差估計 權重* Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.** @min 0.0* @max 0.1* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);/*** Optical flow scale factor* 光流 縮放因子* Factor to scale optical flow** @min 0.0* @max 10.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.35f);/*** Minimal acceptable optical flow quality* 光流質量 下限* 0 - lowest quality, 1 - best quality.** @min 0.0* @max 1.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f);/*** Sonar maximal error for new surface* 超聲波 最大偏差；超過該閾值并穩定，則接受為新平面；【疑似與參數不匹配】* If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable).** @min 0.0* @max 1.0* @unit m* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.2f);/*** Land detector time* 著陸檢測時間* Vehicle assumed landed if no altitude changes happened during this time on low throttle.** @min 0.0* @max 10.0* @unit s* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);/*** Land detector altitude dispersion threshold* 著陸檢測高度閾值* Dispersion threshold for triggering land detector.** @min 0.0* @max 10.0* @unit m* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);/*** Land detector throttle threshold* 著陸檢測 油門閾值* Value should be lower than minimal hovering thrust. Half of it is good choice.** @min 0.0* @max 1.0* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);/*** GPS delay* GPS 延遲補償* GPS delay compensation** @min 0.0* @max 1.0* @unit s* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);/*** Flow module offset (center of rotation) in X direction* ???光流模塊安裝位置（x）偏差* Yaw X flow compensation** @min -1.0* @max 1.0* @unit m* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);/*** Flow module offset (center of rotation) in Y direction* ???光流模塊安裝位置（y）偏差* Yaw Y flow compensation** @min -1.0* @max 1.0* @unit m* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);/*** Mo-cap* 禁用 動作捕捉* Set to 0 if using fake GPS** @value 0 Mo-cap enabled* @value 1 Mo-cap disabled* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0);/*** LIDAR for altitude estimation* lidar 高度估計* @boolean* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_LIDAR_EST, 0);/*** LIDAR calibration offset* lidar 校準偏差* LIDAR calibration offset. Value will be added to the measured distance** @min -20* @max 20* @unit m* @group Position Estimator INAV*/ PARAM_DEFINE_FLOAT(INAV_LIDAR_OFF, 0.0f);/*** Disable vision input* 禁用視覺輸入* Set to the appropriate key (328754) to disable vision input.** @reboot_required true* @min 0* @max 328754* @group Position Estimator INAV*/ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);int inav_parameters_init(struct position_estimator_inav_param_handles *h) {h->w_z_baro = param_find("INAV_W_Z_BARO");h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");h->w_z_lidar = param_find("INAV_W_Z_LIDAR");h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V");h->w_mocap_p = param_find("INAV_W_MOC_P");h->w_xy_flow = param_find("INAV_W_XY_FLOW");h->w_xy_res_v = param_find("INAV_W_XY_RES_V");h->w_gps_flow = param_find("INAV_W_GPS_FLOW");h->w_acc_bias = param_find("INAV_W_ACC_BIAS");h->flow_k = param_find("INAV_FLOW_K");h->flow_q_min = param_find("INAV_FLOW_Q_MIN");h->lidar_err = param_find("INAV_LIDAR_ERR");h->land_t = param_find("INAV_LAND_T");h->land_disp = param_find("INAV_LAND_DISP");h->land_thr = param_find("INAV_LAND_THR");h->no_vision = param_find("CBRK_NO_VISION");h->delay_gps = param_find("INAV_DELAY_GPS");h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");h->disable_mocap = param_find("INAV_DISAB_MOCAP");h->enable_lidar_alt_est = param_find("INAV_LIDAR_EST");h->lidar_calibration_offset = param_find("INAV_LIDAR_OFF");h->att_ext_hdg_m = param_find("ATT_EXT_HDG_M");return 0; }int inav_parameters_update(const struct position_estimator_inav_param_handles *h,struct position_estimator_inav_params *p) {param_get(h->w_z_baro, &(p->w_z_baro));param_get(h->w_z_gps_p, &(p->w_z_gps_p));param_get(h->w_z_gps_v, &(p->w_z_gps_v));param_get(h->w_z_vision_p, &(p->w_z_vision_p));param_get(h->w_z_lidar, &(p->w_z_lidar));param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));param_get(h->w_xy_vision_v, &(p->w_xy_vision_v));param_get(h->w_mocap_p, &(p->w_mocap_p));param_get(h->w_xy_flow, &(p->w_xy_flow));param_get(h->w_xy_res_v, &(p->w_xy_res_v));param_get(h->w_gps_flow, &(p->w_gps_flow));param_get(h->w_acc_bias, &(p->w_acc_bias));param_get(h->flow_k, &(p->flow_k));param_get(h->flow_q_min, &(p->flow_q_min));param_get(h->lidar_err, &(p->lidar_err));param_get(h->land_t, &(p->land_t));param_get(h->land_disp, &(p->land_disp));param_get(h->land_thr, &(p->land_thr));param_get(h->no_vision, &(p->no_vision));param_get(h->delay_gps, &(p->delay_gps));param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));param_get(h->disable_mocap, &(p->disable_mocap));param_get(h->enable_lidar_alt_est, &(p->enable_lidar_alt_est));param_get(h->lidar_calibration_offset, &(p->lidar_calibration_offset));param_get(h->att_ext_hdg_m, &(p->att_ext_hdg_m));return 0; }

4、 position_estimator_inav_main

/*** @file position_estimator_inav_main.c* Model-identification based position estimator for multirotors** @author Anton Babushkin <anton.babushkin@me.com>* @author Nuno Marques <n.marques21@hotmail.com>* @author Christoph Tobler <toblech@student.ethz.ch>*/ #include <px4_posix.h> #include <unistd.h> #include <stdlib.h> #include <stdio.h> #include <stdbool.h> #include <fcntl.h> #include <string.h> #include <px4_config.h> #include <math.h> #include <float.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/att_pos_mocap.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/distance_sensor.h> #include <poll.h> #include <systemlib/err.h> #include <systemlib/mavlink_log.h> #include <geo/geo.h> #include <systemlib/systemlib.h> #include <drivers/drv_hrt.h> #include <platforms/px4_defines.h>#include <terrain_estimation/terrain_estimator.h> #include "position_estimator_inav_params.h" #include "inertial_filter.h"#define MIN_VALID_W 0.00001f //用于檢測是否為零 #define PUB_INTERVAL 10000 // limit publish rate to 100 Hz（發布速度） #define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s（緩存） #define MAX_WAIT_FOR_BARO_SAMPLE 3000000 // wait 3 secs for the baro to respondstatic bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool inav_verbose_mode = false;//在linux中：--verbose 顯示指令執行過程 // 守護進程狀態，守護進程：運行于后臺，并周期性的執行某種任務；//超時設置 static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s（視覺） static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s（動作捕捉） static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s（GPS） static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s（光流） static const hrt_abstime lidar_timeout = 150000; // lidar timeout = 150ms（lidar） static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distance during this time after lidar loss static const unsigned updates_counter_len = 1000000;//更新計數器 步長 static const float max_flow = 1.0f; // max flow value that can be used, rad/s 光流輸出上限extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);//以 c 方式編譯，并輸出函數接口；int position_estimator_inav_thread_main(int argc, char *argv[]);static void usage(const char *reason);//打印命令調用格式信息；static inline int min(int val1, int val2)//最小值 {return (val1 < val2) ? val1 : val2; }static inline int max(int val1, int val2)//最大值 {return (val1 > val2) ? val1 : val2; }/*** Print the correct usage.打印命令調用格式信息；*/ static void usage(const char *reason) {if (reason && *reason) {PX4_INFO("%s", reason);}PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n");return; }/*** The position_estimator_inav_thread only briefly exists to start* the background job. The stack size assigned in the* Makefile does only apply to this management task.** The actual stack size should be set in the call* to task_create().* * position_estimator_inav_thread 進程只是短暫存在，用于啟動后臺進程。在makefile* 中指定的堆棧大小僅用于該管理進程。* 實際的堆棧大小需要在調用 task_create() 時設置。* */ int position_estimator_inav_main(int argc, char *argv[]) {if (argc < 2) {//指令錯誤，并打印正確調用格式；usage("missing command");}if (!strcmp(argv[1], "start")) {//啟動進程if (thread_running) { //判斷是否已經啟動;warnx("already running");/* this is not an error */return 0;}inav_verbose_mode = false; //初始化參數；if ((argc > 2) && (!strcmp(argv[2], "-v"))) {inav_verbose_mode = true; //若有參數 -v，則打印進程詳細信息；}thread_should_exit = false;//px4_task_spawn_cmd()，此處為 POSIX 接口的 任務進程創建函數position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",//進程名稱；SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4600,//任務調度模式，優先級，堆棧大小；position_estimator_inav_thread_main,//進程入口函數；(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);return 0;}// start end；if (!strcmp(argv[1], "stop")) {//進程停止if (thread_running) {warnx("stop");thread_should_exit = true;//設置標識位，在線程中自動跳出 while 循環并結束；} else {warnx("not started");}return 0;}if (!strcmp(argv[1], "status")) {//進程狀態，打印當前進程狀態及用法；if (thread_running) {warnx("is running");} else {warnx("not started");}return 0;}usage("unrecognized command");return 1; }#ifdef INAV_DEBUG //調試打印函數 static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2],float x_est_prev[2], float y_est_prev[2], float z_est_prev[2],float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p,float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) {FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a");if (f) {char *s = malloc(256);unsigned n = snprintf(s, 256,"%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",(unsigned long long)hrt_absolute_time(), msg, (double)dt,(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0],(double)z_est_prev[1]);fwrite(s, 1, n, f);n = snprintf(s, 256,"\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n",(double)acc[0], (double)acc[1], (double)acc[2],(double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1],(double)corr_gps[2][1],(double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0],(double)w_mocap_p);fwrite(s, 1, n, f);n = snprintf(s, 256,"\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n",(double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1],(double)corr_vision[1][1], (double)corr_vision[2][1],(double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v);fwrite(s, 1, n, f);free(s);}fsync(fileno(f));fclose(f); } #else #define write_debug_log(...) //此處 ... 為占位符，用于在 c 語言中實現函數重載。 #endif/***************************************************************************** main****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) {orb_advert_t mavlink_log_pub = nullptr;float x_est[2] = { 0.0f, 0.0f }; // pos, vel [位置，速度]；float y_est[2] = { 0.0f, 0.0f }; // pos, vel 初始化參數為零；float z_est[2] = { 0.0f, 0.0f }; // pos, vel//緩存數據：float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer（位置估計）float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer（旋轉矩陣）float R_gps[3][3]; // rotation matrix for GPS correction moment（當前時刻 用于 GPS 校正的 旋轉矩陣）memset(est_buf, 0, sizeof(est_buf));memset(R_buf, 0, sizeof(R_buf));memset(R_gps, 0, sizeof(R_gps));int buf_ptr = 0;// GPS 水平定位精度參數EPH、垂直定位精度參數EPVstatic const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculationstatic const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation（最大誤差）float eph = max_eph_epv;float epv = 1.0f;float eph_flow = 1.0f;float eph_vision = 0.2f;float epv_vision = 0.2f;float eph_mocap = 0.05f;float epv_mocap = 0.05f;//從上面的參數設置來看，果然精度還是 vicon > 視覺 > 光流，花錢就是D；// 上一時刻的估計值，初始化為0；float x_est_prev[2], y_est_prev[2], z_est_prev[2];memset(x_est_prev, 0, sizeof(x_est_prev));memset(y_est_prev, 0, sizeof(y_est_prev));memset(z_est_prev, 0, sizeof(z_est_prev));int baro_init_cnt = 0;int baro_init_num = 200;float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted// 氣壓計相關參數，200個值求平均，得到氣壓計讀數；hrt_abstime accel_timestamp = 0; //加速度計的時間hrt_abstime baro_timestamp = 0; //氣壓計時間bool ref_inited = false; //參考位置初始化hrt_abstime ref_init_start = 0;const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fixstruct map_projection_reference_s ref;//參考位置，啟動 1s 后初始化該值；memset(&ref, 0, sizeof(ref));uint16_t accel_updates = 0; //用于計算刷新率；uint16_t baro_updates = 0;uint16_t gps_updates = 0;uint16_t attitude_updates = 0;uint16_t flow_updates = 0;uint16_t vision_updates = 0;uint16_t mocap_updates = 0;hrt_abstime updates_counter_start = hrt_absolute_time();//時間，用于計算刷新率hrt_abstime pub_last = hrt_absolute_time(); //時間，用于將當前估計值存入緩存；hrt_abstime t_prev = 0;/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D 導航坐標系float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame 機體坐標系float corr_baro = 0.0f; // D 導航坐標系 Z軸float corr_gps[3][2] = { // 導航坐標系{ 0.0f, 0.0f }, // N (pos, vel){ 0.0f, 0.0f }, // E (pos, vel){ 0.0f, 0.0f }, // D (pos, vel)};float w_gps_xy = 1.0f; //權重float w_gps_z = 1.0f;float corr_vision[3][2] = { // 導航坐標系{ 0.0f, 0.0f }, // N (pos, vel){ 0.0f, 0.0f }, // E (pos, vel){ 0.0f, 0.0f }, // D (pos, vel)};float corr_mocap[3][1] = { // 導航坐標系{ 0.0f }, // N (pos){ 0.0f }, // E (pos){ 0.0f }, // D (pos)};const int mocap_heading = 2; //外部偏航角模式 ，2 表示 動作捕捉系統；float dist_ground = 0.0f; //variables for lidar altitude estimationfloat corr_lidar = 0.0f; //lidar 高度估計相關的變量float lidar_offset = 0.0f;int lidar_offset_count = 0;bool lidar_first = true;bool use_lidar = false;bool use_lidar_prev = false;float corr_flow[] = { 0.0f, 0.0f }; // N E 光流float w_flow = 0.0f;hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered)hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered)int n_flow = 0;float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };float yaw_comp[] = { 0.0f, 0.0f };//偏航補償，光流安裝位置不在中心時引起的偏差；hrt_abstime flow_time = 0; //光流float flow_min_dist = 0.2f; //光流最小距離//各個傳感器是否正常工作；bool gps_valid = false; // GPS is validbool lidar_valid = false; // lidar is validbool flow_valid = false; // flow is validbool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)bool vision_valid = false; // vision is validbool mocap_valid = false; // mocap is valid/* declare and safely initialize all structs */struct actuator_controls_s actuator; //電機控制參數memset(&actuator, 0, sizeof(actuator));struct actuator_armed_s armed; //電機解鎖狀態參數memset(&armed, 0, sizeof(armed));struct sensor_combined_s sensor; //傳感器讀數（陀螺儀、加速度計、磁力計、氣壓計）memset(&sensor, 0, sizeof(sensor));struct vehicle_gps_position_s gps; // GPS 信息memset(&gps, 0, sizeof(gps));struct vehicle_attitude_s att; // 姿態信息memset(&att, 0, sizeof(att));struct vehicle_local_position_s local_pos; //位置信息（NED）memset(&local_pos, 0, sizeof(local_pos));struct optical_flow_s flow; //光流讀數memset(&flow, 0, sizeof(flow));struct vision_position_estimate_s vision; //視覺位置估計memset(&vision, 0, sizeof(vision));struct att_pos_mocap_s mocap; //動作捕捉系統memset(&mocap, 0, sizeof(mocap));struct vehicle_global_position_s global_pos; //以 GPS 為主的位置估計memset(&global_pos, 0, sizeof(global_pos));struct distance_sensor_s lidar; //lidarmemset(&lidar, 0, sizeof(lidar));struct vehicle_rates_setpoint_s rates_setpoint;memset(&rates_setpoint, 0, sizeof(rates_setpoint));/* subscribe 訂閱，返回值為 句柄*/int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);int armed_sub = orb_subscribe(ORB_ID(actuator_armed));int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));/* advertise 發布，返回值為 句柄*/orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);orb_advert_t vehicle_global_position_pub = NULL;// position_estimator_inav_params.h中定義的參數：struct position_estimator_inav_params params;memset(&params, 0, sizeof(params));struct position_estimator_inav_param_handles pos_inav_param_handles;/* initialize parameter handles *///初始化參數句柄；inav_parameters_init(&pos_inav_param_handles);/* first parameters read at start up */struct parameter_update_s param_update;orb_copy(ORB_ID(parameter_update), parameter_update_sub,//初始化更新狀態，沒有實際作用；&param_update); /* read from param topic to clear updated flag *//* first parameters update 根據句柄獲取參數*/inav_parameters_update(&pos_inav_param_handles, &params);px4_pollfd_struct_t fds_init[1] = {};fds_init[0].fd = sensor_combined_sub;//傳感器訂閱句柄->文件設備描述符fds_init[0].events = POLLIN;//有數據可讀/* wait for initial baro value */bool wait_baro = true;TerrainEstimator terrain_estimator;//地形估計，用于估計對地距離；thread_running = true;hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();while (wait_baro && !thread_should_exit) { //while 循環用于 氣壓計數據初始化；int ret = px4_poll(&fds_init[0], 1, 1000);//讀取傳感器，數組維度 1，超時1000ms；if (ret < 0) { // 讀取失敗；/* poll error */mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) { //氣壓計超時，無數據???wait_baro = false;//氣壓計數據讀取失敗，結束while循環；mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");} else if (ret > 0) {if (fds_init[0].revents & POLLIN) {orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);if (wait_baro && sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;baro_wait_for_sample_time = hrt_absolute_time();/* mean calculation over several measurements */if (baro_init_cnt < baro_init_num) {//累加200個氣壓計讀數求均值；if (PX4_ISFINITE(sensor.baro_alt_meter)) {baro_offset += sensor.baro_alt_meter;baro_init_cnt++;}} else {wait_baro = false;//結束while循環，完成氣壓計高度初始化；baro_offset /= (float) baro_init_cnt;//氣壓計高度；local_pos.z_valid = true; //氣壓計數據有效；local_pos.v_z_valid = true;}}}} else {PX4_WARN("INAV poll timeout");}}/* main loop */px4_pollfd_struct_t fds[1];fds[0].fd = vehicle_attitude_sub;//訂閱姿態信息；fds[0].events = POLLIN;//POLLIN：Data other than high-priority data may be read without blocking.；//poll函數參閱：http://pubs.opengroup.org/onlinepubs/007908799/xsh/poll.htmlwhile (!thread_should_exit) {int ret = px4_poll(fds, 1, 20); //超時20ms，即最小刷新率 50Hzhrt_abstime t = hrt_absolute_time();if (ret < 0) { //數據讀取失敗；/* poll error */mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");continue;} else if (ret > 0) { //數據讀取成功，開始訂閱；/* vehicle attitude */orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);attitude_updates++;bool updated;/* parameter update 檢查參數是否有更新*/orb_check(parameter_update_sub, &updated);if (updated) { //獲取新參數；struct parameter_update_s update;orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);inav_parameters_update(&pos_inav_param_handles, &params);}/* actuator */orb_check(actuator_sub, &updated);if (updated) { // actuator_controls_0 執行器控制信息；orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);}//該控制參數輸出到mixer；/* armed */orb_check(armed_sub, &updated);if (updated) { //解鎖信息更新；orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);}/* sensor combined */orb_check(sensor_combined_sub, &updated);if (updated) { //傳感器數據更新；orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {//當前數據未處理，處理后該值相等；if (att.R_valid) {//姿態中旋轉矩陣有效；/* correct accel bias *///加速度計原始數據（m/s^2），減去偏移量；sensor.accelerometer_m_s2[0] -= acc_bias[0];sensor.accelerometer_m_s2[1] -= acc_bias[1];sensor.accelerometer_m_s2[2] -= acc_bias[2];/* transform acceleration vector from body frame to NED frame */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];// PX4_R() 用于提取旋轉矩陣對應元素；}}//將加速度計的向量轉換到 NED 坐標系；acc[2] += CONSTANTS_ONE_G;//重力加速度補償；} else {//旋轉矩陣尚未賦值時；memset(acc, 0, sizeof(acc));}accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;accel_updates++;}if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {//當前數據未處理，處理后該值相等；corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];//高度差 = 起飛點高度 - 氣壓計當前高度 - z軸高度（負）baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;baro_updates++;}}/* lidar alt estimation */orb_check(distance_sensor_sub, &updated);/* update lidar separately, needed by terrain estimator */if (updated) {//傳感器數據更新；orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);lidar.current_distance += params.lidar_calibration_offset;} //獲取 lidar 讀數，并補償偏移量。if (updated) { //check if altitude estimation for lidar is enabled and new sensor dataif (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance&& lidar.current_distance < lidar.max_distance&& (PX4_R(att.R, 2, 2) > 0.7f)) {//R[3,3]=cos(橫滾角)*cos(俯仰)>0.7；表示橫滾與俯仰 小于45°；if (!use_lidar_prev && use_lidar) {//等待 use_lidar=true;prev=false;lidar_first = true;}use_lidar_prev = use_lidar;lidar_time = t;dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distanceif (lidar_first) {lidar_first = false;lidar_offset = dist_ground + z_est[0];//獲取 lidar 偏差；mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset");warnx("[inav] LIDAR: new ground offset");}corr_lidar = lidar_offset - dist_ground - z_est[0];//由 lidar 得到的高度差；if (fabsf(corr_lidar) > params.lidar_err) { //check for spikecorr_lidar = 0; //檢查野值；lidar_valid = false;lidar_offset_count++;if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinitlidar_first = true; //穩定的偏差變化->到達新地形->重新初始化;lidar_offset_count = 0;}} else {//lidar 數據有效；corr_lidar = lidar_offset - dist_ground - z_est[0];lidar_valid = true;lidar_offset_count = 0;lidar_valid_time = t;}} else {lidar_valid = false;}}/* optical flow 光流*/orb_check(optical_flow_sub, &updated);if (updated && lidar_valid) {//lidar 可用；orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);flow_time = t;float flow_q = flow.quality / 255.0f;//歸一化；float dist_bottom = lidar.current_distance;if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {/* distance to surface *///float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonarfloat flow_dist = dist_bottom; //use this if using lidar//lidar 已進行坐標轉換；/* check if flow if too large for accurate measurements *//* calculate estimated velocity in body frame */float body_v_est[2] = { 0.0f, 0.0f };for (int i = 0; i < 2; i++) {//x、y軸的速度，坐標轉換body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];}/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;// V_xy / height，相差較大時，可近似為角速度w，與 roll/pitch 相減后，檢查是否超出光流圖像刷新率. [sin(0)~tan(0)~0]/*calculate offset of flow-gyro using already calibrated gyro from autopilot*///積分值 / 積分時間 = 角速度；（時間單位us？？）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;//moving averageif (n_flow >= 100) {//根據校準后的姿態att，獲取光流陀螺儀的偏差；//濾波后數據：經過 100 點數據低通濾波獲得；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];//數據恢復，準備下一次校準；n_flow = 0;flow_gyrospeed_filtered[0] = 0.0f;flow_gyrospeed_filtered[1] = 0.0f;flow_gyrospeed_filtered[2] = 0.0f;att_gyrospeed_filtered[0] = 0.0f;att_gyrospeed_filtered[1] = 0.0f;att_gyrospeed_filtered[2] = 0.0f;} else {//對 flow_gyrospeed光流陀螺儀角速度 低通濾波；flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);//對 姿態角速度 低通濾波同上；att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);n_flow++;}/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*///光流安裝位置造成的偏差補償；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]);/* convert raw flow to angular flow (rad/s) */float flow_ang[2];/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */orb_check(vehicle_rate_sp_sub, &updated);if (updated) {//角速度設定值orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);}float rate_threshold = 0.15f;//約8.6°if (fabsf(rates_setpoint.pitch) < rate_threshold) {//warnx("[inav] test ohne comp");//機身轉動較小時，不做補償；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)//flow_k 光流縮放因子；flow_ang[0]光流像素 x軸角速度} else {//warnx("[inav] test mit comp");//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)//機身俯仰超過閾值，使用陀螺儀補償其轉動；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;//for now the flow has to be scaled (to small)}if (fabsf(rates_setpoint.roll) < rate_threshold) {//注釋同上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)} else {//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)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;//for now the flow has to be scaled (to small)}/* flow measurements vector */float flow_m[3];if (fabsf(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[2] = z_est[1];/* velocity in NED */float flow_v[2] = { 0.0f, 0.0f };/* project measurements vector to NED basis, skip Z component *///將光流在水平面的測量值 映射到導航坐標系（NED）；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];}}/* velocity correction *///光流的偏差校正量；corr_flow[0] = flow_v[0] - x_est[1];corr_flow[1] = flow_v[1] - y_est[1];/* adjust correction weight */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);//w_flow = cos(俯仰)* cos(橫滾)* flow_q_weight 權重 / 高度；/* if flow is not accurate, reduce weight for it */// TODO make this more fuzzyif (!flow_accurate) {//飛太快，相機跟不上；w_flow *= 0.05f;}/* under ideal conditions, on 1m distance assume EPH = 10cm */eph_flow = 0.1f / w_flow;//根據 w_flow 確定水平精度；flow_valid = true;} else {//光流條件惡劣，數據無效；w_flow = 0.0f;flow_valid = false;}flow_updates++;}/* check no vision circuit breaker is set */if (params.no_vision != CBRK_NO_VISION_KEY) {//啟用視覺輸入；/* vehicle vision position */orb_check(vision_position_estimate_sub, &updated);if (updated) {orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);static float last_vision_x = 0.0f;static float last_vision_y = 0.0f;static float last_vision_z = 0.0f;/* reset position estimate on first vision update */if (!vision_valid) {//只執行一次；x_est[0] = vision.x;x_est[1] = vision.vx;y_est[0] = vision.y;y_est[1] = vision.vy;/* only reset the z estimate if the z weight parameter is not zero */if (params.w_z_vision_p > MIN_VALID_W) {z_est[0] = vision.z;z_est[1] = vision.vz;}vision_valid = true;last_vision_x = vision.x;last_vision_y = vision.y;last_vision_z = vision.z;warnx("VISION estimate valid");mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");}/* calculate correction for position *///位移：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];static hrt_abstime last_vision_time = 0;float vision_dt = (vision.timestamp - last_vision_time) / 1e6f;last_vision_time = vision.timestamp;if (vision_dt > 0.000001f && vision_dt < 0.2f) {//時間大于0，小于0.2vision.vx = (vision.x - last_vision_x) / vision_dt;vision.vy = (vision.y - last_vision_y) / vision_dt;vision.vz = (vision.z - last_vision_z) / vision_dt;last_vision_x = vision.x;last_vision_y = vision.y;last_vision_z = vision.z;/* calculate correction for velocity *///速度差；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];} else {/* assume zero motion *///假設沒有發生運動；corr_vision[0][1] = 0.0f - x_est[1];corr_vision[1][1] = 0.0f - y_est[1];corr_vision[2][1] = 0.0f - z_est[1];}vision_updates++;}}/* vehicle mocap position *///動作捕捉系統， 與視覺相似；orb_check(att_pos_mocap_sub, &updated);if (updated) {orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);if (!params.disable_mocap) {/* reset position estimate on first mocap update */if (!mocap_valid) {x_est[0] = mocap.x;y_est[0] = mocap.y;z_est[0] = mocap.z;mocap_valid = true;warnx("MOCAP data valid");mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid");}/* calculate correction for position */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];mocap_updates++;}}/* vehicle GPS position */orb_check(vehicle_gps_position_sub, &updated);if (updated) {orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);bool reset_est = false;/* hysteresis for GPS quality */if (gps_valid) {if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {//fix_type < 3 無法定位，或無法定位 3D 信息；gps_valid = false;mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");warnx("[inav] GPS signal lost");}} else {if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {//信號良好；gps_valid = true;reset_est = true;mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");warnx("[inav] GPS signal found");}}if (gps_valid) {//GPS分辨率，參考msg信息；double lat = gps.lat * 1e-7;double lon = gps.lon * 1e-7;float alt = gps.alt * 1e-3;/* initialize reference position if needed */if (!ref_inited) {if (ref_init_start == 0) {ref_init_start = t;} else if (t > ref_init_start + ref_init_delay) {ref_inited = true;/* set position estimate to (0, 0, 0), use GPS velocity for XY */x_est[0] = 0.0f;x_est[1] = gps.vel_n_m_s;y_est[0] = 0.0f;y_est[1] = gps.vel_e_m_s;local_pos.ref_lat = lat;local_pos.ref_lon = lon;local_pos.ref_alt = alt + z_est[0];//地表高度；local_pos.ref_timestamp = t;/* initialize projection *///轉為弧度，賦值給ref；map_projection_init(&ref, lat, lon);// XXX replace this printwarnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);}}if (ref_inited) {/* project GPS lat lon to plane */float gps_proj[2];map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));//GPS的坐標轉換，沒看懂；/* reset position estimate when GPS becomes good */if (reset_est) {//GPS 信號良好；x_est[0] = gps_proj[0];x_est[1] = gps.vel_n_m_s;y_est[0] = gps_proj[1];y_est[1] = gps.vel_e_m_s;}/* calculate index of estimated values in buffer */int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));if (est_i < 0) {est_i += EST_BUF_SIZE;}/* calculate correction for position *///由 GPS 得到的位移；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];/* calculate correction for velocity */if (gps.vel_ned_valid) {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];} else {corr_gps[0][1] = 0.0f;corr_gps[1][1] = 0.0f;corr_gps[2][1] = 0.0f;}/* save rotation matrix at this moment */memcpy(R_gps, R_buf[est_i], sizeof(R_gps));w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);}} else {//GPS 搜不到星；/* no GPS lock */memset(corr_gps, 0, sizeof(corr_gps));ref_init_start = 0;}gps_updates++;}}/* check for timeout on FLOW topic 檢查各個傳感器數據是否超時*/if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {flow_valid = false;warnx("FLOW timeout");mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout");}/* check for timeout on GPS topic */if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) {gps_valid = false;warnx("GPS timeout");mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");}/* check for timeout on vision topic */if (vision_valid && (t > (vision.timestamp + vision_topic_timeout))) {vision_valid = false;warnx("VISION timeout");mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");}/* check for timeout on mocap topic */if (mocap_valid && (t > (mocap.timestamp + mocap_topic_timeout))) {mocap_valid = false;warnx("MOCAP timeout");mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");}/* check for lidar measurement timeout */if (lidar_valid && (t > (lidar_time + lidar_timeout))) {lidar_valid = false;warnx("LIDAR timeout");mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout");}float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 mst_prev = t;/* increase EPH/EPV on each step *///定位精度隨時間漂移，直到下次校準；if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0eph = 0.001;}if (eph < max_eph_epv) {eph *= 1.0f + dt;}if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0epv = 0.001;}if (epv < max_eph_epv) {epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)}//根據設定參數和數據質量，決定是否使用傳感器值；/* use GPS if it's valid and reference position initialized */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;/* use VISION if it's valid and has a valid weight parameter */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;/* use MOCAP if it's valid and has a valid weight parameter */bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W&& params.att_ext_hdg_m == mocap_heading; //check if external heading is mocapif (params.disable_mocap) { //disable mocap if fake gps is useduse_mocap = false;}/* use flow if it's valid and (accurate or no GPS available) */bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);/* use LIDAR if it's valid and lidar altitude estimation is enabled */use_lidar = lidar_valid && params.enable_lidar_alt_est;bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);//調整各個傳感器的權重；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;/* reduce GPS weight if optical flow is good */if (use_flow && flow_accurate) {w_xy_gps_p *= params.w_gps_flow;w_xy_gps_v *= params.w_gps_flow;}/* baro offset correction */if (use_gps_z) {//此處 高度差 為低通濾波，corr_gps首先對時間積分得到高度，在乘以權重w；float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;baro_offset += offs_corr;corr_baro += offs_corr;}/* accelerometer bias correction for GPS (use buffered rotation matrix) */float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };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;}/* transform error vector from NED frame to body frame */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;}//accelerometer_m_s2[] -= acc_bias[];完成加速度計校準（濾波？）；}/* accelerometer bias correction for VISION (use buffered rotation matrix) */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;}/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */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;}/* transform error vector from NED frame to body frame */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;}}/* accelerometer bias correction for flow and baro (assume that there is no delay) */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;}/* transform error vector from NED frame to body frame */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;}}/* inertial filter prediction for altitude *///使用加速度值，預測速度和位移；inertial_filter_predict(dt, z_est, acc[2]);if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);memcpy(z_est, z_est_prev, sizeof(z_est));}/* inertial filter correction for altitude */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 (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);memcpy(z_est, z_est_prev, sizeof(z_est));memset(corr_gps, 0, sizeof(corr_gps));memset(corr_vision, 0, sizeof(corr_vision));memset(corr_mocap, 0, sizeof(corr_mocap));corr_baro = 0;} else {memcpy(z_est_prev, z_est, sizeof(z_est));}if (can_estimate_xy) {/* inertial filter prediction for position */inertial_filter_predict(dt, x_est, acc[0]);inertial_filter_predict(dt, y_est, acc[1]);if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);memcpy(x_est, x_est_prev, sizeof(x_est));memcpy(y_est, y_est_prev, sizeof(y_est));}/* inertial filter correction for position */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 + 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);}if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);memcpy(x_est, x_est_prev, sizeof(x_est));memcpy(y_est, y_est_prev, sizeof(y_est));memset(corr_gps, 0, sizeof(corr_gps));memset(corr_vision, 0, sizeof(corr_vision));memset(corr_mocap, 0, sizeof(corr_mocap));memset(corr_flow, 0, sizeof(corr_flow));} else {memcpy(x_est_prev, x_est, sizeof(x_est));memcpy(y_est_prev, y_est, sizeof(y_est));}} else {/* gradually reset xy velocity estimates *///當 xy軸位置 無法估計時，逐漸將速度歸零; inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);}/* run terrain estimator *///地形估計;（對地高度）（卡爾曼濾波）terrain_estimator.predict(dt, &att, &sensor, &lidar);//根據加速度信息預測；terrain_estimator.measurement_update(hrt_absolute_time(), &gps, &lidar, &att);//根據 GPS 和 lidar 校正預測信息;if (inav_verbose_mode) {//打印詳細信息/* print updates rate */if (t > updates_counter_start + updates_counter_len) {float updates_dt = (t - updates_counter_start) * 0.000001f;warnx("updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",(double)(accel_updates / updates_dt),(double)(baro_updates / updates_dt),(double)(gps_updates / updates_dt),(double)(attitude_updates / updates_dt),(double)(flow_updates / updates_dt),(double)(vision_updates / updates_dt),(double)(mocap_updates / updates_dt));updates_counter_start = t;accel_updates = 0;baro_updates = 0;gps_updates = 0;attitude_updates = 0;flow_updates = 0;vision_updates = 0;mocap_updates = 0;}}if (t > pub_last + PUB_INTERVAL) {pub_last = t;/* push current estimate to buffer */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];/* push current rotation matrix to buffer */memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));buf_ptr++;if (buf_ptr >= EST_BUF_SIZE) {buf_ptr = 0;}/* publish local position *///將位置和速度信息置入 local_pos 準備發布；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];// z軸 正方向向下;}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) {/* publish global position */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);//將 NED 坐標信息映射到 GPS 坐標;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;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);}}}}warnx("stopped");mavlink_log_info(&mavlink_log_pub, "[inav] stopped");thread_running = false;return 0; } 版權聲明：本文為博主原創文章，轉載請注明，歡迎交流~

總結

以上是生活随笔為你收集整理的PX4 - position_estimator_inav的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。