PX4 - position_estimator_inav

by luoshi006

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

0、 概述

几种位置估计

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

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

3. 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_GROUP	Q Estimator	INAV	LPE	EKF2
0	1	1
1	1		1
2				1

INAV 原理

预测函数：

s+=vt+12at2 s + = v t + 1 2 a t 2

v+=at v + = a t

校正部分：

没看懂。。。。。
这部分应该是在用 二阶低通滤波 ，不过具体过程并没有推导出来，数学基础有待提升啊。。。

【有兴趣的同学可以从二阶低通滤波的复频域形式进行推导】，也希望有思路的同学不吝赐教~~~

另：

主程序中，在最后也大量使用了这种滤波，对加速度偏差信息进行校正；
不过默认参数 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 
#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 
 *
 * 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 
 * @author Nuno Marques 
 * @author Christoph Tobler 
 */
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include 
#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 respond

static 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、垂直定位精度参数EPV
    static const float min_eph_epv = 2.0f;  // min EPH/EPV, used for weight calculation
    static 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 fix
    struct 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 estimation
    float 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 valid
    bool lidar_valid = false;       // lidar is valid
    bool flow_valid = false;        // flow is valid
    bool flow_accurate = false;     // flow should be accurate (this flag not updated if flow_valid == false)
    bool vision_valid = false;      // vision is valid
    bool 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;         //lidar
    memset(&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(¶ms, 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,//初始化更新状态，没有实际作用；
         ¶m_update); /* read from param topic to clear updated flag */
    /* first parameters update  根据句柄获取参数*/
    inav_parameters_update(&pos_inav_param_handles, ¶ms);

    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.html

    while (!thread_should_exit) {
        int ret = px4_poll(fds, 1, 20); //超时20ms，即最小刷新率 50Hz
        hrt_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, ¶ms);
            }

            /* 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 data

                if (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 distance

                    if (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 spike
                        corr_lidar = 0;     //检查野值；
                        lidar_valid = false;
                        lidar_offset_count++;

                        if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit
                            lidar_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 sonar
                    float 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 average
                    if (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 fuzzy
                    if (!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.2
                        vision.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 print
                            warnx("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 ms
        t_prev = t;

        /* increase EPH/EPV on each step */
        //定位精度随时间漂移，直到下次校准；
        if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
            eph = 0.001;
        }

        if (eph < max_eph_epv) {
            eph *= 1.0f + dt;
        }

        if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
            epv = 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 mocap

        if (params.disable_mocap) { //disable mocap if fake gps is used
            use_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;
}