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

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

0、 概述

幾種位置估計

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

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

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

位置估計切換

配置 SYS_MC_EST_GROUP：

SYS_MC_EST_GROUPQ EstimatorINAVLPEEKF2

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

INAV 原理

預測函數(shù)：

s+=vt+12at2

v+=at

校正部分：

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

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

另：

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

發(fā)布信息

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 對應的發(fā)布信息是 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軸 權(quán)重（截止頻率）* 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軸 權(quán)重（截止頻率）* 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軸速度 權(quán)重（截止頻率）* 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軸 權(quán)重（截止頻率）* 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軸 權(quán)重（截止頻率）* 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軸位置 權(quán)重（截止頻率）* 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軸速度 權(quán)重（截止頻率）* 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軸位置 權(quán)重（截止頻率）* 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軸速度 權(quán)重（截止頻率）* 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* 動作捕捉系統(tǒng) 位置 權(quán)重* 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軸 權(quán)重* 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軸 權(quán)重* When velocity sources lost slowly decrease estimated horizontal velocity with this weight.* 速度信號丟失后，依此權(quán)重緩慢減少水平面的速度估計值；* @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軸 權(quán)重因子* 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* 加速度計 偏差估計 權(quán)重* 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* 光流質(zhì)量 下限* 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* 超聲波 最大偏差；超過該閾值并穩(wěn)定，則接受為新平面；【疑似與參數(shù)不匹配】* 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（發(fā)布速度） #define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s（緩存） #define MAX_WAIT_FOR_BARO_SAMPLE 3000000 // wait 3 secs for the baro to respondstatic bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool inav_verbose_mode = false;//在linux中：--verbose 顯示指令執(zhí)行過程 // 守護進程狀態(tài)，守護進程：運行于后臺，并周期性的執(zhí)行某種任務；//超時設置 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;//更新計數(shù)器 步長 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 方式編譯，并輸出函數(shù)接口；int position_estimator_inav_thread_main(int argc, char *argv[]);static void usage(const char *reason);//打印命令調(diào)用格式信息；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.打印命令調(diào)用格式信息；*/ 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* 中指定的堆棧大小僅用于該管理進程。* 實際的堆棧大小需要在調(diào)用 task_create() 時設置。* */ int position_estimator_inav_main(int argc, char *argv[]) {if (argc < 2) {//指令錯誤，并打印正確調(diào)用格式；usage("missing command");}if (!strcmp(argv[1], "start")) {//啟動進程if (thread_running) { //判斷是否已經(jīng)啟動;warnx("already running");/* this is not an error */return 0;}inav_verbose_mode = false; //初始化參數(shù)；if ((argc > 2) && (!strcmp(argv[2], "-v"))) {inav_verbose_mode = true; //若有參數(shù) -v，則打印進程詳細信息；}thread_should_exit = false;//px4_task_spawn_cmd()，此處為 POSIX 接口的 任務進程創(chuàng)建函數(shù)position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",//進程名稱；SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4600,//任務調(diào)度模式，優(yōu)先級，堆棧大小；position_estimator_inav_thread_main,//進程入口函數(shù)；(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 循環(huán)并結(jié)束；} else {warnx("not started");}return 0;}if (!strcmp(argv[1], "status")) {//進程狀態(tài)，打印當前進程狀態(tài)及用法；if (thread_running) {warnx("is running");} else {warnx("not started");}return 0;}usage("unrecognized command");return 1; }#ifdef INAV_DEBUG //調(diào)試打印函數(shù) 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 語言中實現(xiàn)函數(shù)重載。 #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 初始化參數(shù)為零；float z_est[2] = { 0.0f, 0.0f }; // pos, vel//緩存數(shù)據(jù)：float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer（位置估計）float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer（旋轉(zhuǎn)矩陣）float R_gps[3][3]; // rotation matrix for GPS correction moment（當前時刻 用于 GPS 校正的 旋轉(zhuǎn)矩陣）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 水平定位精度參數(shù)EPH、垂直定位精度參數(shù)EPVstatic const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculationstatic const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation（最大誤差）float eph = max_eph_epv;float epv = 1.0f;float eph_flow = 1.0f;float eph_vision = 0.2f;float epv_vision = 0.2f;float eph_mocap = 0.05f;float epv_mocap = 0.05f;//從上面的參數(shù)設置來看，果然精度還是 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// 氣壓計相關(guān)參數(shù)，200個值求平均，得到氣壓計讀數(shù)；hrt_abstime accel_timestamp = 0; //加速度計的時間hrt_abstime baro_timestamp = 0; //氣壓計時間bool ref_inited = false; //參考位置初始化hrt_abstime ref_init_start = 0;const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fixstruct map_projection_reference_s ref;//參考位置，啟動 1s 后初始化該值；memset(&ref, 0, sizeof(ref));uint16_t accel_updates = 0; //用于計算刷新率；uint16_t baro_updates = 0;uint16_t gps_updates = 0;uint16_t attitude_updates = 0;uint16_t flow_updates = 0;uint16_t vision_updates = 0;uint16_t mocap_updates = 0;hrt_abstime updates_counter_start = hrt_absolute_time();//時間，用于計算刷新率hrt_abstime pub_last = hrt_absolute_time(); //時間，用于將當前估計值存入緩存；hrt_abstime t_prev = 0;/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D 導航坐標系float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame 機體坐標系float corr_baro = 0.0f; // D 導航坐標系 Z軸float corr_gps[3][2] = { // 導航坐標系{ 0.0f, 0.0f }, // N (pos, vel){ 0.0f, 0.0f }, // E (pos, vel){ 0.0f, 0.0f }, // D (pos, vel)};float w_gps_xy = 1.0f; //權(quán)重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 表示 動作捕捉系統(tǒng)；float dist_ground = 0.0f; //variables for lidar altitude estimationfloat corr_lidar = 0.0f; //lidar 高度估計相關(guān)的變量float lidar_offset = 0.0f;int lidar_offset_count = 0;bool lidar_first = true;bool use_lidar = false;bool use_lidar_prev = false;float corr_flow[] = { 0.0f, 0.0f }; // N E 光流float w_flow = 0.0f;hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered)hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered)int n_flow = 0;float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };float yaw_comp[] = { 0.0f, 0.0f };//偏航補償，光流安裝位置不在中心時引起的偏差；hrt_abstime flow_time = 0; //光流float flow_min_dist = 0.2f; //光流最小距離//各個傳感器是否正常工作；bool gps_valid = false; // GPS is validbool lidar_valid = false; // lidar is validbool flow_valid = false; // flow is validbool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)bool vision_valid = false; // vision is validbool mocap_valid = false; // mocap is valid/* declare and safely initialize all structs */struct actuator_controls_s actuator; //電機控制參數(shù)memset(&actuator, 0, sizeof(actuator));struct actuator_armed_s armed; //電機解鎖狀態(tài)參數(shù)memset(&armed, 0, sizeof(armed));struct sensor_combined_s sensor; //傳感器讀數(shù)（陀螺儀、加速度計、磁力計、氣壓計）memset(&sensor, 0, sizeof(sensor));struct vehicle_gps_position_s gps; // GPS 信息memset(&gps, 0, sizeof(gps));struct vehicle_attitude_s att; // 姿態(tài)信息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; //光流讀數(shù)memset(&flow, 0, sizeof(flow));struct vision_position_estimate_s vision; //視覺位置估計memset(&vision, 0, sizeof(vision));struct att_pos_mocap_s mocap; //動作捕捉系統(tǒng)memset(&mocap, 0, sizeof(mocap));struct vehicle_global_position_s global_pos; //以 GPS 為主的位置估計memset(&global_pos, 0, sizeof(global_pos));struct distance_sensor_s lidar; //lidarmemset(&lidar, 0, sizeof(lidar));struct vehicle_rates_setpoint_s rates_setpoint;memset(&rates_setpoint, 0, sizeof(rates_setpoint));/* subscribe 訂閱，返回值為 句柄*/int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);int armed_sub = orb_subscribe(ORB_ID(actuator_armed));int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));/* advertise 發(fā)布，返回值為 句柄*/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中定義的參數(shù)：struct position_estimator_inav_params params;memset(&params, 0, sizeof(params));struct position_estimator_inav_param_handles pos_inav_param_handles;/* initialize parameter handles *///初始化參數(shù)句柄；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,//初始化更新狀態(tài)，沒有實際作用；&param_update); /* read from param topic to clear updated flag *//* first parameters update 根據(jù)句柄獲取參數(shù)*/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;//有數(shù)據(jù)可讀/* 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 循環(huán)用于 氣壓計數(shù)據(jù)初始化；int ret = px4_poll(&fds_init[0], 1, 1000);//讀取傳感器，數(shù)組維度 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) { //氣壓計超時，無數(shù)據(jù)???wait_baro = false;//氣壓計數(shù)據(jù)讀取失敗，結(jié)束while循環(huán)；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個氣壓計讀數(shù)求均值；if (PX4_ISFINITE(sensor.baro_alt_meter)) {baro_offset += sensor.baro_alt_meter;baro_init_cnt++;}} else {wait_baro = false;//結(jié)束while循環(huán)，完成氣壓計高度初始化；baro_offset /= (float) baro_init_cnt;//氣壓計高度；local_pos.z_valid = true; //氣壓計數(shù)據(jù)有效；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;//訂閱姿態(tài)信息；fds[0].events = POLLIN;//POLLIN：Data other than high-priority data may be read without blocking.；//poll函數(shù)參閱：http://pubs.opengroup.org/onlinepubs/007908799/xsh/poll.htmlwhile (!thread_should_exit) {int ret = px4_poll(fds, 1, 20); //超時20ms，即最小刷新率 50Hzhrt_abstime t = hrt_absolute_time();if (ret < 0) { //數(shù)據(jù)讀取失敗；/* poll error */mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");continue;} else if (ret > 0) { //數(shù)據(jù)讀取成功，開始訂閱；/* vehicle attitude */orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);attitude_updates++;bool updated;/* parameter update 檢查參數(shù)是否有更新*/orb_check(parameter_update_sub, &updated);if (updated) { //獲取新參數(shù)；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 執(zhí)行器控制信息；orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);}//該控制參數(shù)輸出到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) { //傳感器數(shù)據(jù)更新；orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {//當前數(shù)據(jù)未處理，處理后該值相等；if (att.R_valid) {//姿態(tài)中旋轉(zhuǎn)矩陣有效；/* correct accel bias *///加速度計原始數(shù)據(jù)（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() 用于提取旋轉(zhuǎn)矩陣對應元素；}}//將加速度計的向量轉(zhuǎn)換到 NED 坐標系；acc[2] += CONSTANTS_ONE_G;//重力加速度補償；} else {//旋轉(zhuǎn)矩陣尚未賦值時；memset(acc, 0, sizeof(acc));}accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;accel_updates++;}if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {//當前數(shù)據(jù)未處理，處理后該值相等；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) {//傳感器數(shù)據(jù)更新；orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);lidar.current_distance += params.lidar_calibration_offset;} //獲取 lidar 讀數(shù)，并補償偏移量。if (updated) { //check if altitude estimation for lidar is enabled and new sensor dataif (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance&& lidar.current_distance < lidar.max_distance&& (PX4_R(att.R, 2, 2) > 0.7f)) {//R[3,3]=cos(橫滾角)*cos(俯仰)>0.7；表示橫滾與俯仰 小于45°；if (!use_lidar_prev && use_lidar) {//等待 use_lidar=true;prev=false;lidar_first = true;}use_lidar_prev = use_lidar;lidar_time = t;dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distanceif (lidar_first) {lidar_first = false;lidar_offset = dist_ground + z_est[0];//獲取 lidar 偏差；mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset");warnx("[inav] LIDAR: new ground offset");}corr_lidar = lidar_offset - dist_ground - z_est[0];//由 lidar 得到的高度差；if (fabsf(corr_lidar) > params.lidar_err) { //check for spikecorr_lidar = 0; //檢查野值；lidar_valid = false;lidar_offset_count++;if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinitlidar_first = true; //穩(wěn)定的偏差變化->到達新地形->重新初始化;lidar_offset_count = 0;}} else {//lidar 數(shù)據(jù)有效；corr_lidar = lidar_offset - dist_ground - z_est[0];lidar_valid = true;lidar_offset_count = 0;lidar_valid_time = t;}} else {lidar_valid = false;}}/* optical flow 光流*/orb_check(optical_flow_sub, &updated);if (updated && lidar_valid) {//lidar 可用；orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);flow_time = t;float flow_q = flow.quality / 255.0f;//歸一化；float dist_bottom = lidar.current_distance;if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {/* distance to surface *///float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonarfloat flow_dist = dist_bottom; //use this if using lidar//lidar 已進行坐標轉(zhuǎn)換；/* 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軸的速度，坐標轉(zhuǎn)換body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];}/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;// V_xy / height，相差較大時，可近似為角速度w，與 roll/pitch 相減后，檢查是否超出光流圖像刷新率. [sin(0)~tan(0)~0]/*calculate offset of flow-gyro using already calibrated gyro from autopilot*///積分值 / 積分時間 = 角速度；（時間單位us？？）flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;//moving averageif (n_flow >= 100) {//根據(jù)校準后的姿態(tài)att，獲取光流陀螺儀的偏差；//濾波后數(shù)據(jù)：經(jīng)過 100 點數(shù)據(jù)低通濾波獲得；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];//數(shù)據(jù)恢復，準備下一次校準；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);//對 姿態(tài)角速度 低通濾波同上；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");//機身轉(zhuǎn)動較小時，不做補償；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)//機身俯仰超過閾值，使用陀螺儀補償其轉(zhuǎn)動；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 權(quán)重 / 高度；/* if flow is not accurate, reduce weight for it */// TODO make this more fuzzyif (!flow_accurate) {//飛太快，相機跟不上；w_flow *= 0.05f;}/* under ideal conditions, on 1m distance assume EPH = 10cm */eph_flow = 0.1f / w_flow;//根據(jù) w_flow 確定水平精度；flow_valid = true;} else {//光流條件惡劣，數(shù)據(jù)無效；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) {//只執(zhí)行一次；x_est[0] = vision.x;x_est[1] = vision.vx;y_est[0] = vision.y;y_est[1] = vision.vy;/* only reset the z estimate if the z weight parameter is not zero */if (params.w_z_vision_p > MIN_VALID_W) {z_est[0] = vision.z;z_est[1] = vision.vz;}vision_valid = true;last_vision_x = vision.x;last_vision_y = vision.y;last_vision_z = vision.z;warnx("VISION estimate valid");mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");}/* calculate correction for position *///位移：corr_vision[0][0] = vision.x - x_est[0];corr_vision[1][0] = vision.y - y_est[0];corr_vision[2][0] = vision.z - z_est[0];static hrt_abstime last_vision_time = 0;float vision_dt = (vision.timestamp - last_vision_time) / 1e6f;last_vision_time = vision.timestamp;if (vision_dt > 0.000001f && vision_dt < 0.2f) {//時間大于0，小于0.2vision.vx = (vision.x - last_vision_x) / vision_dt;vision.vy = (vision.y - last_vision_y) / vision_dt;vision.vz = (vision.z - last_vision_z) / vision_dt;last_vision_x = vision.x;last_vision_y = vision.y;last_vision_z = vision.z;/* calculate correction for velocity *///速度差；corr_vision[0][1] = vision.vx - x_est[1];corr_vision[1][1] = vision.vy - y_est[1];corr_vision[2][1] = vision.vz - z_est[1];} else {/* assume zero motion *///假設沒有發(fā)生運動；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 *///動作捕捉系統(tǒng)， 與視覺相似；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 *///轉(zhuǎn)為弧度，賦值給ref；map_projection_init(&ref, lat, lon);// XXX replace this printwarnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);}}if (ref_inited) {/* project GPS lat lon to plane */float gps_proj[2];map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));//GPS的坐標轉(zhuǎn)換，沒看懂；/* 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 檢查各個傳感器數(shù)據(jù)是否超時*/if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {flow_valid = false;warnx("FLOW timeout");mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout");}/* check for timeout on GPS topic */if (gps_valid && (t > (gps.timestamp + gps_topic_timeout))) {gps_valid = false;warnx("GPS timeout");mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");}/* check for timeout on vision topic */if (vision_valid && (t > (vision.timestamp + vision_topic_timeout))) {vision_valid = false;warnx("VISION timeout");mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");}/* check for timeout on mocap topic */if (mocap_valid && (t > (mocap.timestamp + mocap_topic_timeout))) {mocap_valid = false;warnx("MOCAP timeout");mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");}/* check for lidar measurement timeout */if (lidar_valid && (t > (lidar_time + lidar_timeout))) {lidar_valid = false;warnx("LIDAR timeout");mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout");}float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 mst_prev = t;/* increase EPH/EPV on each step *///定位精度隨時間漂移，直到下次校準；if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0eph = 0.001;}if (eph < max_eph_epv) {eph *= 1.0f + dt;}if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0epv = 0.001;}if (epv < max_eph_epv) {epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)}//根據(jù)設定參數(shù)和數(shù)據(jù)質(zhì)量，決定是否使用傳感器值；/* use GPS if it's valid and reference position initialized */bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;/* use VISION if it's valid and has a valid weight parameter */bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;/* use MOCAP if it's valid and has a valid weight parameter */bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W&& params.att_ext_hdg_m == mocap_heading; //check if external heading is mocapif (params.disable_mocap) { //disable mocap if fake gps is useduse_mocap = false;}/* use flow if it's valid and (accurate or no GPS available) */bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);/* use LIDAR if it's valid and lidar altitude estimation is enabled */use_lidar = lidar_valid && params.enable_lidar_alt_est;bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);//調(diào)整各個傳感器的權(quán)重；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首先對時間積分得到高度，在乘以權(quán)重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) {//二階低通濾波，根據(jù)位置和速度偏差，得到加速度偏差，用于加速度計校正;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);//根據(jù)加速度信息預測；terrain_estimator.measurement_update(hrt_absolute_time(), &gps, &lidar, &att);//根據(jù) 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 準備發(fā)布；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);//發(fā)布位置信息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; } 版權(quán)聲明：本文為博主原創(chuàng)文章，轉(zhuǎn)載請注明，歡迎交流~

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。