程序员经验分享-hwhale

PX4 - position_estimator_inav

2023-05-16

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_GROUPQ EstimatorINAVLPEEKF2
011
111
21

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

PX4

position

estimator

inav

PX4 - position_estimator_inav 的相关文章

  • 在Java中模拟鼠标在非活动窗口的某个位置单击？

    不管怎样 我正在构建一个在后台运行的机器人 这个机器人需要我点击 当然 我希望能够在机器人运行时做其他事情 所以我想知道是否可以在非活动窗口的某个位置模拟鼠标单击 如果可能的话 如果你们中有人能帮助我 我将不胜感激 java awt Rob

  • 带有位置参数的 Git 别名

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

  • CSS：如何摆脱默认窗口“填充”？设置为 100% 宽度的元素不会到达窗口边框

    所以我有一个直接放置在 body 内部的元素 div Some stuff div Other stuff 以下是使用的CSS body text align center header margin auto 因此 header div

  • gsub 或 scan 中的匹配位置

    实现匹配位置的最佳方法是什么 由 对于使用时的每场比赛gsub or scan hello gsub Regexp last match offset 0 first gt 01234 See 正则表达式 last match http w

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

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

  • jquery查找元素的位置

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

  • 锚链接着陆位置错误

    可能是一个愚蠢的问题 但老实说我无法理解这里出了什么问题 http harrisonfjord com thinkinc http harrisonfjord com thinkinc 我目前正在建设的网站 我想在以下位置创建一个锚链接ht

  • 获取UIScrollView的当前位置

    我来自 Android 但在 IOS 上我感到很头疼 我需要制作一个像电影片尾字幕一样的滚动菜单 我使用了下面的代码 rol scroll view contentOffset y timer NSTimer scheduledTimerW

  • 当页面滚动并有某个位置时执行某些操作

    我在页面滚动方面遇到了一些问题 当我向下滚动页面并且窗口距顶部位置 100px 时 我想更改一些 css 这是我的代码 非常感谢您的帮助 document ready function window scroll function if w

  • 子div高度100%内部位置：固定div + 自动溢出

    我在尝试以下操作时遇到一些奇怪的行为 请参阅jsfiddle http jsfiddle net 9nS47 http jsfiddle net 9nS47 HTML div div div div div div div div div

  • 在同一位置绘制顶点

    有没有一种方法可以在同一位置绘制两个图的共享节点 例如 两张图 g1 graph ring 5 V g1 name c node1 node2 node3 node4 node5 g1 g1 V g1 1 g2 graph ring 5 V

  • 数组切片返回 [object Object] 而不是值

    我试图获取特定 div 被删除时的位置 在一些帮助下 我整理了下面的代码 我在最后一点添加了尝试获取特定值 但它只是返回 object Object 而不是 0 0 或 0 120 之类的东西 那么问题是如何从数组中获取实际值 Here h

  • 使用position_dodge绘制geom_segment

    我有一个数据集 其中包含个人随时间的工作地点信息 更具体地说 我有关于个人在特定工作场所工作的时间间隔的信息 library tidyverse library lubridate individual A a id lt c rep A

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

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

  • 当父div在屏幕上不可见时，jQuery UI位置函数问题

    我在 jQuery UI Position 函数方面遇到了一个奇怪的问题 有一个父 div 大于屏幕高度 其中还有另一个小 div 我的函数告诉小 div 位于其父级的底部 当底部可见时 一切正常 但是当由于窗口大小而导致底部不可见时 位置

  • 如何固定/锁定背景图像和包含图像的 Div 的位置

    我有一个地图图像 1080x1080px 我希望它作为主体或容器 div 的背景 我需要图像始终保持固定在其位置 即使在调整浏览器窗口大小时也是如此 我在主 div 容器内有 div 这些 div 包含图像 这些图像是放置在特定位置的地图标

  • MPL pos 是一个未记录的元函数吗？

    里面有下面的示例代码BOOST MPL 文档find算法 http www boost org doc libs 1 46 1 libs mpl doc refmanual find html typedef vector

  • 翻转旋转和图像

    我正在用 Java 编写一个平台游戏 并且正在手动编码玩家动画 我分别为每个肢体设置动画 改变位置和旋转 当玩家面向右时 这工作得很好 但是当玩家面向左时 我不知道如何处理旋转 以使它们在玩家向左转时看起来相同 每个身体部位的位置都是相对于

  • 如何在具有动态高度的固定 div 标题后设置 div 内容样式

    以下情况 div style width 100 place holder for header div div style width 100 margin top 100px content div 我需要标题始终可见且位于顶部 因此该

  • 为什么当我滚动父项时，position().top 会发生变化？

    jQuery position http api jquery com position returns 匹配集合中第一个元素的当前坐标 元素 相对于偏移父级 所以 滚动父级是不应该改变立场 right 我得到的结果这把小提琴 http j

随机推荐

  • 用于异常检测的深度神经网络模型融合

    用于异常检测的深度神经网络模型融合 在当今的数字时代 xff0c 网络安全至关重要 xff0c 因为全球数十亿台计算机通过网络连接 近年来 xff0c 网络攻击的数量大幅增加 因此 xff0c 网络威胁检测旨在通过观察一段时间内的流量数据来

  • STM32CubeIDE构建通用freertos项目（一）

    感慨 本人大约三四年没有碰单片机了 xff0c 遥想当年我还是用的keil工具 有幸以援助的身份介入公司的嵌入式项目 xff0c 结合自身经验讲讲 工作是一个长期的过程 xff0c 开头不注意则会产生蝴蝶效应 xff0c 导致接下来的工作一

  • CMake中CMakeLists文件的编写以及变量打印

    最近在学习PCL xff0c 在Win10下使用VS编写PCL程序 xff0c 配置环境时经常出错 xff0c 踩坑记录详见 xff1a Win10 43 VS2017 43 PCL 1 8 1软件安装 踩坑记录 看到 点云库PCL从入门到

  • 如何离线安装python包

    在我们的日常使用python的过程中 xff0c 通常是通过联网安装相关的依赖包 xff0c 但是有时候会有一些情况是没有网络的 xff0c 但我们又需要安装python的各种包 而包的依赖导致我们很难一个一个地从pypi网站下载whl文件

  • mavros 环境配置注意事项[Resource not found: px4 ROS path ]

    背景 我最近开始使用mavros xff0c 按照官网的教程进行安装 采用二进制的方式安装 xff0c 一切进行的很顺利 xff0c 接下来我就按照PX4官网的去执行一个让无人机自动起飞的例子 xff0c 完全按照官网的代码和配置到最后一步

  • rosdep init ERROR: cannot download default sources list... 解决方法

    问题描述 如标题所示 xff0c 当我们安装好ROS后 xff0c 想要用rosdep初始化时 xff0c 会遇到ERROR cannot download default sources list from https raw githu

  • Linux 运行命令时修改.bashrc并结束命令时恢复原样

    问题来源 我有一个bash程序 xff0c 想要在执行该程序的时候修 bashrc xff0c 然后更新一些环境变量 xff0c 并在结束 ctrl 43 c 的时候再把程序恢复原样 操作如下 xff1a echo 命令把想要增加的内容写入

  • 数字信号处理实验(一)

    实验目的 本次实验目的为 xff1a 在matlab环境下产生几种基本的数字信号 xff0c 并对这些基本的信号进行运算和变换 xff0c 同时利用程序结果对采样定理进行验证 xff0c 深刻理解采样定理 通过自己录制音频信号并对不同的音频

  • 数字图像处理实验(二)

    实验目的 实验一 xff1a 图像增强 了解图像增强的目的及意义 xff0c 加深对图像增强的感性认知 1 掌握直接灰度变换的图像增强方法 2 掌握灰度直方图的概念及其计算方法 3 掌握直方图均衡化合直方图规定化得计算过程 实验二 xff1

  • 信息论实验-信源编码2(Lz编码和算数编码的C++实现)

    上一篇文章给出了Huffman编码和Shannon Fano编码的编码原理以及C 43 43 的程序 xff0c 程序可以用来实现给任意类型的文件进行无损压缩 xff0c 缺点是比较耗时 xff0c 不能作为正常的通用压缩软件来使用 xff

  • 信息论实验-二元对称信道仿真(C++实现)

    二元对称信道模拟器 实验目的 加深理解二进制对称信道的工作原理 xff0c 掌握通过高级编程语言生成伪随机数的方法 允许使用编程语言 xff1a C xff0c C 43 43 等 实验要求 输入 xff1a BSC信道的错误概率 xff0

  • OpenCV 安装必看

    怎样安装OpenCV套件呢 xff1f 想要使用opencv的同学一定是刚刚接触到图像处理 xff0c 需要做一些实验 xff0c 听说OpenCV很好用 xff0c 所以就开始查找各种资料学习OpenCV但是 xff0c 谁告诉你们它很好

  • make: *** No rule to make target `menuconfig'. Stop.问题解决方案-Linux（3）

    前言 本问题是我在编译更新内核时所遇到的 xff0c 已经解决 问题描述 在编译内核时 xff0c 运行make menuconfig 时出现 xff0c 截图如下 这个是因为没有找到要配置的文件 解决方案 进入解压得到的Linux原文件夹

  • 深入理解AlexNet网络

    AlexNet 论文 xff1a ImageNet Classification with Deep Convolutional Neural Networks 第一个典型的CNN是LeNet5网络结构 xff0c 但是第一个引起大家注意的

  • Ubuntu 18.04 网络配置

    坑爹的网络配置 ubuntu 18 04的网络配置的方式相较于原来的版本有了很大的改动 xff0c 并且server版的和Desktop 版本的是不一样的 Server版本 新的版本采用了netplan 管理网络 xff0c 在命令行中配置

  • PCA原理

    PCA 各位 xff0c 久违了 xff5e 什么是PCA xff1f 什么是PCA呢 xff1f 这是一个问题 xff0c 什么样的问题 xff1f 简单而又复杂的问题 xff0c 简单是因为百度一下就会出现一大堆的解释 xff0c 复杂

  • SRAM驱动开发实例

    一 我写博客的原因 xff0c 应该说是有两点吧 xff08 1 xff09 一点是对阶段性工作的总结 xff0c 虽说技术创新 xff0c 技术创新 xff0c 但在创新之前有一个技术积累的过程 xff0c 写博客 xff0c 便于总结

  • 互补滤波器

    互补滤波器 从 RC 电路 到 数字滤波器 参考 xff1a wikiPedia by luoshi006 原理 低通滤波器 一阶低通滤波器 传递函数 常见的 RC 电路构成的一阶低通滤波器的输入 U 输出 Y 关系如下 xff1a Y U

  • mahony 互补滤波器

    by luoshi006 上接 互补滤波器 xff0c 继续学习互补滤波 参考 xff1a Keeping a Good Attitude A Quaternion Based Orientation Filter for IMUs and

  • PX4 - position_estimator_inav

    by luoshi006 参考 xff1a 1 http dev px4 io advanced switching state estimators html 2 http blog sina com cn s blog 8fe4f2f4

热门标签