px4源码学习（local position estimator）_px4源码学习(local position estimator)

前言：之前学习的是px4源码中的attitude_estimator_q，可以说是学习的相当仔细和深入。于是借着这股劲继续学习位置估计中的local position estimator,另外需要说明的是，在px4源码中还有一个位置估计是position_estimator_inav，这个是基于导航的位置估计。

---

一、文件夹下有什么？

我们先看看文件夹下有什么，如图：

说明：
1.sensors文件夹下是进行位置估计时用到的模块：气压，光流，gps，mocap，超声波，视觉等的初始化和卡尔曼滤波的修正函数，这些函数在BlockLocalPositionEstimator.cpp中被调用。

2.BlockLocalPositionEstimator.cpp和BlockLocalPositionEstimator.h是BlockLocalPositionEstimator这个类的定义，.h里面是类的成员，cpp里面是成员的定义，这个类就是lpe的核心，后面专门用一节讲这个类。

3.local_position_estimator_main.cpp， 这个文件是nuttx操作系统的一个应用程序，它启动local_position_estimator_thread_main这个任务，这个任务函数的代码比较短。

---

二、sensor文件夹介绍

上面说过，sensors文件夹下是进行位置估计时用到的模块，用到的模块有气压，光流，gps，mocap，超声波，视觉，这些cpp文件的写法风格和函数名称都比较类似，这里选择其中一个mocap进行介绍，因为mocap中三维的消息都有，其他超声波只有在高度，光流只有平面位置，gps只有在室外才能用，视觉的门槛高，精度又不高。

在mocap中只有4个函数（其他模块也一样）。

mocapInit：

在第一次连续接受到20帧mocap消息就任务初始化完毕，这个函数在上电后第一次使用或者mocap消息超时以后会被调用。mocap消息超时指连续200ms没有接收到mocap消息，这时px4认为mocap消息断开了，会重新调用初始化函数重新开始。在实际测试过程中，由于通信的原因，偶尔的两帧消息之间超过200ms导致的超时不会产生很大的影响，一般在1s之内它能够重新初始化，或者把源码中的REQ_MOCAP_INIT_COUNT改小也可以使偶尔因为通信带来的超时造成的影响变小。

mocapMeasure：

这函数把订阅到的mocap消息赋值到卡尔曼的测量矩阵，并记录收到mocap消息的时间

mocapCorrect：

卡尔曼修正函数，里面是数学矩阵的运算，具体可以查阅卡尔曼滤波相关内容。

mocapCheckTimeout：

检查是否超时。

---

三、BlockLocalPositionEstimator类

这个类中以publish开头的函数是把估计出的位置通过uORB发送出去。

在讲下面两个函数前先简单说说卡尔曼的函数实现，其实就4个字，“预测”，“修正”，lpe用加速度计信息和飞机当前姿态进行位置的预测，用sensors文件夹的各个模块进行修正。

predict函数：这个函数利用当前的加速度计信息和当前飞机的姿态进行位置的预测。使用了runge kutta methods 的卡尔曼的公式。

update函数中调用了上一节中各个模块的修正函数，比如mocapCorrect，当然在调用前有一些判断机制，看mocap消息是否有更新，是否启用mocap，是否超时等。还调用了预测函数predict。

其他的一些函数是对卡尔曼滤波矩阵的赋值，有些相当于滤波参数，可以直接在地面站进行修改。这里用的卡尔曼滤波的A矩阵大小是10*10

---

四、具体的源码

#include "BlockLocalPositionEstimator.hpp"
#include <drivers/drv_hrt.h> //之前介绍过，和rtc有关
#include <systemlib/mavlink_log.h>
#include <fcntl.h> //与文件控制操作有关，fcntl=file control
#include <systemlib/err.h>
#include <matrix/math.hpp>
#include <cstdlib>  //cstdlib是C++里面的一个常用函数库， 等价于C中的<stdlib.h>。

orb_advert_t mavlink_log_pub = nullptr;

// required standard deviation of estimate for estimator to publish data
// 发布的数据估计量所要求的的标准偏差
static const uint32_t       EST_STDDEV_XY_VALID = 2.0;  // x、y方向的标准位置偏差2.0 m
static const uint32_t       EST_STDDEV_Z_VALID = 2.0;   // z方向的标准位置偏差2.0 m
static const uint32_t       EST_STDDEV_TZ_VALID = 2.0;  // 地面的海拔高度的标准偏差2.0 m

static const float P_MAX = 1.0e6f;      // 规定状态协方差的最大值
static const float LAND_RATE = 10.0f;   // rate of land detector correction

static const char *msg_label = "[lpe] ";


//下面是构造函数，从24行到167行都是初始化列表
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :

    // this block has no parent, and has name LPE
    SuperBlock(nullptr, "LPE"),    //用于制定父类的名字和该block的名字，因为该block没有父类所以为空指针
    ModuleParams(nullptr),         //c++里面的标准类，用去其他类使用配置参数


    // 下面是用于订阅相关的信息，设置订阅速率，并添加到列表
    _sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
    _sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
    _sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),


    // set flow max update rate higher than expected to we don't lose packets
    // 订阅光流传感器的信息
    _sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),


    // 订阅sensor_combined信息
    _sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),


    // status updates 2 hz
    // 系统参数的更新
    _sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),


    // gps 10 hz
    // 订阅gps位置信息
    _sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),


    // vision 50 hz
    // 订阅视觉位置信息
    _sub_vision_pos(ORB_ID(vehicle_vision_position), 1000 / 50, 0, &getSubscriptions()),


    // mocap 50 hz
    //订阅mocap信息
    _sub_mocap(ORB_ID(att_pos_mocap), 1000 / 50, 0, &getSubscriptions()),



    // all distance sensors, 10 hz
    // 订阅所有的距离传感器信息，比如雷达、声呐等等
    _sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
    _sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
    _sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
    _sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
    _dist_subs(),
    _sub_lidar(nullptr),
    _sub_sonar(nullptr),
    _sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()),



    // publications
    // 配置发布信息发布
    _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
    _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
    _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
    _pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),



    // map projection
    // 地图规划，里面有很多东西，这里主要是初始经纬度
    _map_ref(),

    // flow gyro
    // 设置 flow gyro filter
    _flow_gyro_x_high_pass(this, "FGYRO_HP"),
    _flow_gyro_y_high_pass(this, "FGYRO_HP"),

    // stats
    // 配置统计功能
    _baroStats(this, ""),
    _sonarStats(this, ""),
    _lidarStats(this, ""),
    _flowQStats(this, ""),
    _visionStats(this, ""),
    _mocapStats(this, ""),
    _gpsStats(this, ""),

    // low pass
    // 配置低通滤波
    _xLowPass(this, "X_LP"),


    // 配置针对agl的低通滤波
    _aglLowPass(this, "X_LP"),

    // delay
    // 配置延迟
    _xDelay(this, ""),
    _tDelay(this, ""),

    // misc
    // 把这些时间该归零的归零，该设置成绝对时间的设置成绝对时间
    _polls(),
    _timeStamp(hrt_absolute_time()),
    _time_origin(0),
    _timeStampLastBaro(hrt_absolute_time()),
    _time_last_hist(0),
    _time_last_flow(0),
    _time_last_baro(0),
    _time_last_gps(0),
    _time_last_lidar(0),
    _time_last_sonar(0),
    _time_init_sonar(0),
    _time_last_vision_p(0),
    _time_last_mocap(0),
    _time_last_land(0),
    _time_last_target(0),

    // reference altitudes
    // 配置参考点的海拔及初始化情况

    _altOrigin(0),
    _altOriginInitialized(false),
    _altOriginGlobal(false),
    _baroAltOrigin(0),
    _gpsAltOrigin(0),

    // status
    // 配置gps、armed状态
    _receivedGps(false),
    _lastArmedState(false),

    // masks
    _sensorTimeout(UINT16_MAX),
    _sensorFault(0),
    _estimatorInitialized(0),

    // sensor update flags
    // 初始化传感器的更新标识符
    _flowUpdated(false),
    _gpsUpdated(false),
    _visionUpdated(false),
    _mocapUpdated(false),
    _lidarUpdated(false),
    _sonarUpdated(false),
    _landUpdated(false),
    _baroUpdated(false)

/*******************下面是构造函数的具体内容*********************/
{

        //把订阅的距离信息放进数组里面
    _dist_subs[0] = &_sub_dist0;
    _dist_subs[1] = &_sub_dist1;
    _dist_subs[2] = &_sub_dist2;
    _dist_subs[3] = &_sub_dist3;

        // 配置轮询光流信息、参数更新、传感器信息的参数
    _polls[POLL_FLOW].fd = _sub_flow.getHandle();
    _polls[POLL_FLOW].events = POLLIN;

    _polls[POLL_PARAM].fd = _sub_param_update.getHandle();
    _polls[POLL_PARAM].events = POLLIN;

    _polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
    _polls[POLL_SENSORS].events = POLLIN;


        //初始化 A, B, P, x, u
        _x.setZero();
        _u.setZero();
        initSS();    //这里面包含了A、B、P的初始化

    // map
    _map_ref.init_done = false;

    // print fusion settings to console
        // 将融合的设置信息打印在控制台
    printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
           "landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
           "baro: %d\n",
           (_fusion.get() & FUSE_GPS) != 0,
           (_fusion.get() & FUSE_FLOW) != 0,
           (_fusion.get() & FUSE_VIS_POS) != 0,
           (_fusion.get() & FUSE_LAND_TARGET) != 0,
           (_fusion.get() & FUSE_LAND) != 0,
           (_fusion.get() & FUSE_PUB_AGL_Z) != 0,
           (_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
           (_fusion.get() & FUSE_BARO) != 0);
}

//下面这个dynamics函数就是动态方程，本质上是一个一阶微分方程
Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
    float t,
    const Vector<float, BlockLocalPositionEstimator::n_x> &x,
    const Vector<float, BlockLocalPositionEstimator::n_u> &u)
{
    return _A * x + _B * u;
}


//下面的update函数是重点
//update函数的结构流程是：
//1.先调用了预测函数predict结合加速度计和飞机此时的姿态位置进行预测
//2.调用了各个模块的修正函数，比如mocapCorrect
//当然在调用前有一些判断机制，看mocap消息是否有更新，是否启用mocap，是否超时等

void BlockLocalPositionEstimator::update()
{

    // 这里的poll是用来轮询之前提过的flow、param_update、sensor_combined
    // 轮询周期是100ms
    int ret = px4_poll(_polls, 3, 100);

    if (ret < 0)   //poll<0说明poll机制的返回值函数调用失败，同时会自动设置全局变量erro
    {
        return;
    }

    uint64_t newTimeStamp = hrt_absolute_time();//获取绝对时间
    float dt = (newTimeStamp - _timeStamp) / 1.0e6f;//计算出程序更新周期（us）
    _timeStamp = newTimeStamp;

    // 将计算的dt应用于所有子模块（child block）
    setDt(dt);

    // 检测飞机状态是否解锁，arm代表解锁，disarm代表上锁
    bool armedState = _sub_armed.get().armed;


        if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr))
        //如果没有解锁就读取测量的距离值，这里主要是雷达数据和超声波数据
        {
                //检查距离传感器
                //N_DIST_SUBS代表距离传感器的个数，所以for循环就是挨个挨个检查
                for (int i = 0; i < N_DIST_SUBS; i++)
                {
            uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];

                        if (s == _sub_lidar || s == _sub_sonar)//首先判断距离数据是否来自雷达或者声呐，是就continue，即换下一个传感器
                        {
                            continue;
                        }

                        if (s->updated())  //如果不是来自雷达或者声呐，则更新距离数据
                        {
                                s->update();

                                if (s->get().timestamp == 0)
                                {
                                    continue;
                                }

                                if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
                                     s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
                                    _sub_lidar == nullptr)//if括号里的内容用于判断数据是否来自激光
                                {
                                    _sub_lidar = s;
                                        //向控制台打印mavlink信息
                                     mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %i", msg_label, i);

                                }
                                else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
                                         s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
                                           _sub_sonar == nullptr)//else if括号里的内容用于判断数据是否来自超声波
                                {
                                         _sub_sonar = s;
                                        //向控制台打印mavlink信息
                                         mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %i", msg_label, i);
                                }
                        }
                }
        }

    // reset pos, vel, and terrain on arming

    // XXX this will be re-enabled for indoor use cases using a
    // selection param, but is really not helping outdoors
    // right now.

    // if (!_lastArmedState && armedState) {

    //      // we just armed, we are at origin on the ground
    //      _x(X_x) = 0;
    //      _x(X_y) = 0;
    //      // reset Z or not? _x(X_z) = 0;

    //      // we aren't moving, all velocities are zero
    //      _x(X_vx) = 0;
    //      _x(X_vy) = 0;
    //      _x(X_vz) = 0;

    //      // assume we are on the ground, so terrain alt is local alt
    //      _x(X_tz) = _x(X_z);

    //      // reset lowpass filter as well
    //      _xLowPass.setState(_x);
    //      _aglLowPass.setState(0);
    // }

        _lastArmedState = armedState;

    // see which updates are available
        bool paramsUpdated = _sub_param_update.updated();
        _baroUpdated = false;//气压计更新标识符


        //如果气压计数据有更新
        if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated())
        {
                int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;

                if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID)
                {
                    uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
                    _sub_sensor.get().baro_timestamp_relative;

                    if (baro_timestamp != _timeStampLastBaro)
                    {
                         _baroUpdated = true;
                         _timeStampLastBaro = baro_timestamp;
                    }
                }
        }


        //然后开始订阅各个数据，包括光流数据、gps数据、视觉、传感器数据等等，并获取他们的更新状态，是true还是false
        //疑惑的地方：下面用到二进制码相与的运算符&，是用于什么？和获取更新状态有什么关系？根据前面的内容我猜测大概是指配置是否正确
        //&左边是是得到的配置信息，&右边是规定的配置信息
    _flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
    _gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
    _visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
    _mocapUpdated = _sub_mocap.updated();
    _lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
    _sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
    _landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
    bool targetPositionUpdated = _sub_landing_target_pose.updated();

        // 这个才是订阅数据，上面的部分主要是获取更新状态以及订阅前的uorb_check
        updateSubscriptions();

        // 更新参数
        if (paramsUpdated)
        {
                SuperBlock::updateParams();//更新block中的参数
                ModuleParams::updateParams();//更新module参数，主要是更新一些模块或子模块的首地址
                updateSSParams();//更新R矩阵和Q矩阵
        }



        // 接着判断X坐标和Y坐标数据的有效性
        bool vxy_stddev_ok = false;



        //判断p中关于x、y向的速度方差是否小于是否小于设定值，即反映了vx和vy是否有效
        if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) 
        {
        vxy_stddev_ok = true;
        }


        //判断测量xy向的各个传感器是否都初始化
        if (_estimatorInitialized & EST_XY)//如果都初始化了
        {
                // 但是如果vx、vy无效并且gps超时，还是将Initialized设为未完成
                if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS))
                {
                     _estimatorInitialized &= ~EST_XY;
                }

        }

        else  //else说明xy向的各个传感器并没有都初始化
        {

                if (vxy_stddev_ok)
                {
                        //但是只要有一个模块初始化完成，并且vx和vy有效，都说明初始化完成
                        if (!(_sensorTimeout & SENSOR_GPS)
                               || !(_sensorTimeout & SENSOR_FLOW)
                               || !(_sensorTimeout & SENSOR_VISION)
                               || !(_sensorTimeout & SENSOR_MOCAP)
                               || !(_sensorTimeout & SENSOR_LAND)
                               || !(_sensorTimeout & SENSOR_LAND_TARGET))

                        {
                           _estimatorInitialized |= EST_XY;
                        }
                }
        }

        // 接着判断z坐标数据的有效性，与上面同理
        bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

    if (_estimatorInitialized & EST_Z)
    {
        // if valid and baro has timed out, set to not valid
        if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) 
        {
            _estimatorInitialized &= ~EST_Z;
        }

    } 
    else 
    {
        if (z_stddev_ok) 
        {
            _estimatorInitialized |= EST_Z;
        }
    }



        // 判断地形数据有效性，地面的海拔高度反应地形，与上面同理
    bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();

    if (_estimatorInitialized & EST_TZ) {
        if (!tz_stddev_ok) {
            _estimatorInitialized &= ~EST_TZ;
        }

    } else {
        if (tz_stddev_ok) {
            _estimatorInitialized |= EST_TZ;
        }
    }



        // 检查超时情况
        checkTimeouts();


        // 开始更新GPS数据
        // 如果validxy==true并且未设置初始经纬度，则将类的初始化列表中的经纬度（即前面提到的_map_ref）
        // 设为设为初始化经纬度
        if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get())
        {
                //将类的初始化列表中的经纬度（即前面提到的_map_ref）设为初始化经纬度
        map_projection_init(&_map_ref,
                    _init_origin_lat.get(),
                    _init_origin_lon.get());

        // set timestamp when origin was set to current time
        _time_origin = _timeStamp;

        mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
                         double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
    }


        // 重新初始化x数据
        bool reinit_x = false; //重新初始化x数据的标识符为false

        for (int i = 0; i < n_x; i++) //x数据总共有十个
        {
        // should we do a reinit
        // of sensors here?
        // don't want it to take too long
                if (!PX4_ISFINITE(_x(i))) //只要有一个x数据发散，就需要将重新初始化x数据的标识符设为true
                                          //然后跳出for循环执行重新初始化操作，其实也就是全设为零
                                          //而如果x数据不发散，由于reinit_x = false，就不会执行下面的初始化操作
                {
                     reinit_x = true;
                     mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
                     break;
                }
        }

        if (reinit_x)
        {
                for (int i = 0; i < n_x; i++)
                {
                        _x(i) = 0;//将10个x数据初始化为0
                }

                mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
        }

        //与上面同理，检查p里面数据的有效性
        bool reinit_P = false;

        for (int i = 0; i < n_x; i++)
        {
                for (int j = 0; j <= i; j++)
                {
                        if (!PX4_ISFINITE(_P(i, j)))
                        {
                             mavlink_and_console_log_info(&mavlink_log_pub,
                                 "%sreinit P (%d, %d) not finite", msg_label, i, j);
                             reinit_P = true;
                        }

                        if (i == j)
                        {
                // make sure diagonal elements are positive
                                if (_P(i, i) <= 0)
                                {
                                      mavlink_and_console_log_info(&mavlink_log_pub,
                                     "%sreinit P (%d, %d) negative", msg_label, i, j);
                                      reinit_P = true;
                                }

                        }
                        else
                        {
                                _P(j, i) = _P(i, j);//利用p的对称性
                        }

                        if (reinit_P)
                        {
                            break;
                        }
                }

                if (reinit_P)
                {
                    break;
                }
        }

        if (reinit_P)
        {
            initP();
        }

        // 预测，即状态预估计
        predict();

        // 下面就是结合传感器以及其他模块对预测值进行补偿校正
        if (_gpsUpdated) //如果用的是gps
        {
                if (_sensorTimeout & SENSOR_GPS)//如果超时
                {
                        gpsInit();//gps初始化

                }

                else
                {
                        gpsCorrect();//没有超时就校正
                }
        }

        if (_baroUpdated) //如果用的是baro
        {
                if (_sensorTimeout & SENSOR_BARO) //如果超时
                {
                        baroInit();//baro初始化

                }

                else
                {
                        baroCorrect();//没有超时就校正
                }
        }



        if (_lidarUpdated) //如果用的是雷达
        {
                if (_sensorTimeout & SENSOR_LIDAR)//如果超时
                {
                        lidarInit();//雷达初始化
                }

                else
                {
                        lidarCorrect();//没有超时就校正
                }
        }


        if (_sonarUpdated) //如果用的是声呐
        {
                if (_sensorTimeout & SENSOR_SONAR) //如果超时
                {
                        sonarInit();//声呐初始化
                }

                else
                {
                        sonarCorrect();//没有超时就校正
                }
        }


        if (_flowUpdated)//如果用的是光流
        {
                if (_sensorTimeout & SENSOR_FLOW) //如果超时
                {
                        flowInit();//光流初始化
                }

                else
                {
                        flowCorrect();//没有超时就校正
                }
        }


        if (_visionUpdated) //如果用的是视觉
        {
                if (_sensorTimeout & SENSOR_VISION) //如果超时
                {
                        visionInit();//视觉初始化
                }

                else
                {
                        visionCorrect();//没有超时就校正
                }
        }


        if (_mocapUpdated) //如果用的是mocap
        {
                if (_sensorTimeout & SENSOR_MOCAP) //如果超时
                {
                        mocapInit();//视觉初始化
                }

                else
                {
                        mocapCorrect();//没有超时就校正
                }
        }


        if (_landUpdated) //如果用的是land
        {
                if (_sensorTimeout & SENSOR_LAND) //如果超时
                {
                        landInit();//land初始化
                }
                else
                {
                        landCorrect();//没有超时就校正
                }
        }


        if (targetPositionUpdated) //如果用的是landing_target
        {
                if (_sensorTimeout & SENSOR_LAND_TARGET) //如果超时
                {
                        landingTargetInit();//初始化
                }

                else
                {
                        landingTargetCorrect();//没有超时就校正
                }
        }


  /****************发布信息*****************/
        if (_altOriginInitialized)
        {
        // update all publications if possible
                publishLocalPos();//发布local position
                publishEstimatorStatus();//发布十个状态量
                _pub_innov.get().timestamp = _timeStamp;//发布时间
                _pub_innov.update(); //更新公告、订阅、发布的句柄

                if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get()))
                {
                        publishGlobalPos();//发布global position
                }
        }




        //更新延迟信息
        //注意：下面的dt_hist是接收传感器信息的时间间隔，而前面在update函数
        //最开始定义的dt是执行update函数的时间周期，注意区分两者
        float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);//本次的时间减去上一次记录的时间就是间隔（us）

        if (_time_last_hist == 0 ||(dt_hist > HIST_STEP)) //前面定义过最大的间隔为HIST_STEP = 0.05s
        {
        _tDelay.update(Scalar<uint64_t>(_timeStamp));
        _xDelay.update(_x);
        _time_last_hist = _timeStamp;
        }
}


/**检查是否超时**/
void BlockLocalPositionEstimator::checkTimeouts()
{
    baroCheckTimeout();
    gpsCheckTimeout();
    lidarCheckTimeout();
    flowCheckTimeout();
    sonarCheckTimeout();
    visionCheckTimeout();
    mocapCheckTimeout();
    landCheckTimeout();
    landingTargetCheckTimeout();
}


/****判断是否着陆****/
//判断的方法：根据vehicle_land_detected这个topic
//这个topic中有bool landed、bool freefall、bool ground_contact、bool maybe_landed这四个布尔量
bool BlockLocalPositionEstimator::landed()
{
        if (!(_fusion.get() & FUSE_LAND))
        {
           return false;
        }

        bool disarmed_not_falling = (!_sub_armed.get().armed) && (!_sub_land.get().freefall);

        return _sub_land.get().landed || disarmed_not_falling;
}



/**发布本地位置**/
void BlockLocalPositionEstimator::publishLocalPos()
{
    const Vector<float, n_x> &xLP = _xLowPass.getState();

    // lie about eph/epv to allow visual odometry only navigation when velocity est. good
        float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));//求vx、vy的标准偏差
        float epv = sqrtf(_P(X_z, X_z));//计算垂直位置误差的标准差
        float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));//计算水平位置误差的标准差
        float eph_thresh = 3.0f;//eph的threshold（阈值）
        float epv_thresh = 3.0f;//epv的threshold（阈值）

        //根据两个阈值限定eph和epv
        if (vxy_stddev < _vxy_pub_thresh.get())
        {
                if (eph > eph_thresh)
                {
                   eph = eph_thresh;
                }

                if (epv > epv_thresh)
                {
                   epv = epv_thresh;
                }
        }


    // publish local position
    if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
        PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
            && PX4_ISFINITE(_x(X_vz)))  //先判断数据是否发散
        {
                _pub_lpos.get().timestamp = _timeStamp;//发布时间点
                _pub_lpos.get().xy_valid = _estimatorInitialized & EST_XY;  //发布xy的距离数据是否有效
                _pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;    //发布z的距离数据是否有效
                _pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;//发布xy的速度数据是否有效
                _pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z;  //发布z的速度数据是否有效
                _pub_lpos.get().x = xLP(X_x);   //发布x（北）方向的距离信息
                _pub_lpos.get().y = xLP(X_y);   //发布y（东）方向的距离信息

                if (_fusion.get() & FUSE_PUB_AGL_Z) //判断是要发布相对高度还是绝对高度
                {
                        _pub_lpos.get().z = -_aglLowPass.getState()；//发布相对地面的高度（agl）
                                                                    //因为坐标系为东北地，所以要加负号
                }

                else
                {
                        _pub_lpos.get().z = xLP(X_z);   // down 发布绝对高度
                }

                _pub_lpos.get().vx = xLP(X_vx); // 发布x（北）方向的速度
                _pub_lpos.get().vy = xLP(X_vy); // 发布y（东）方向的速度
                _pub_lpos.get().vz = xLP(X_vz); // 发布z（下）方向的速度

        // this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
                _pub_lpos.get().z_deriv = xLP(X_vz); //发布z方向的位置变化率，用X_vz代替

                _pub_lpos.get().yaw = _eul(2);//发布偏航角速度
                _pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;//发布xy方向的global position信息的有效性
                _pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;//发布z方向的global position信息的有效性
                _pub_lpos.get().ref_timestamp = _time_origin;//发布更新gps时的时间点，_time_origin在前面gps数据更新时出现过
                _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;//将纬度信息转换为°的形式发布出去
                _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;//将经度信息转换为°的形式发布出去
                _pub_lpos.get().ref_alt = _altOrigin;//发布原点（参考点）的海拔高度
                _pub_lpos.get().dist_bottom = _aglLowPass.getState();//发布飞机下表面到地面的距离信息
                _pub_lpos.get().dist_bottom_rate = -xLP(X_vz);//发布向下的距离变化率



        // we estimate agl even when we don't have terrain info
        // if you are in terrain following mode this is important
        // so that if terrain estimation fails there isn't a
        // sudden altitude jump
                _pub_lpos.get().dist_bottom_valid = _estimatorInitialized & EST_Z;//发布向下的距离信息的有效性
                _pub_lpos.get().eph = eph;   //发布水平位置误差的标准差
                _pub_lpos.get().epv = epv;   //发布垂直位置误差的标准差
                _pub_lpos.update();
        //TODO provide calculated values for these
                _pub_lpos.get().evh = 0.0f;   //发布水平速度误差的标准差=0
                _pub_lpos.get().evv = 0.0f;   //发布垂直速度误差的标准差=0
                _pub_lpos.get().vxy_max = 0.0f;  //注意：vxy指水平这个合成速度，这里=0不是指水平速度=0，而是指没有用到这个量
                                                 //从vehicle_local_position.msg里得知的
                _pub_lpos.get().limit_hagl = false;
        }
}


/**发布估计的状态**/
void BlockLocalPositionEstimator::publishEstimatorStatus()
{
        _pub_est_status.get().timestamp = _timeStamp;//发布时间点

    for (int i = 0; i < n_x; i++)
    {
        _pub_est_status.get().states[i] = _x(i);//发布十个状态量
        _pub_est_status.get().covariances[i] = _P(i, i);//发布误差协方差矩阵p
    }

        _pub_est_status.get().n_states = n_x;     //发布状态量的个数，这里为10
        _pub_est_status.get().nan_flags = 0;      //用于检验状态数据是否为无穷量
        _pub_est_status.get().health_flags = _sensorFault;  //发布传感器的健康状况
        _pub_est_status.get().timeout_flags = _sensorTimeout; //发布表明超时的flag
        _pub_est_status.get().pos_horiz_accuracy = _pub_gpos.get().eph;//相对于原点的水平位移
        _pub_est_status.get().pos_vert_accuracy = _pub_gpos.get().epv; //相对于原点的垂直位移

        _pub_est_status.update();
}


/**发布global position**/
void BlockLocalPositionEstimator::publishGlobalPos()
{
    // publish global position
    double lat = 0;
    double lon = 0;
    const Vector<float, n_x> &xLP = _xLowPass.getState();
    map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);//这就是之前提到的函数，将平面坐标转换为球坐标
    float alt = -xLP(X_z) + _altOrigin; //绝对高度=地面海拔+相对高度，因为xLP(X_z)是负值，所以加负号转为正

    // lie about eph/epv to allow visual odometry only navigation when velocity est. good
    float vxy_stddev = sqrtf(_P(X_vx, X_vx) + _P(X_vy, X_vy));//计算水平速度的标准偏差
    float epv = sqrtf(_P(X_z, X_z));  //计算垂直位置误差的标准差
    float eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); //计算水平位置误差的标准差
    float eph_thresh = 3.0f; //eph的阈值
    float epv_thresh = 3.0f; //epv的阈值


        //根据两个阈值限定eph和epv，跟前面的发布local position一样
        if (vxy_stddev < _vxy_pub_thresh.get())
        {
                if (eph > eph_thresh)
                {
                   eph = eph_thresh;
                }

                if (epv > epv_thresh)
                {
                   epv = epv_thresh;
                }
        }

    if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
        PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
            PX4_ISFINITE(xLP(X_vz))) //判断数据是否发散
    {
                _pub_gpos.get().timestamp = _timeStamp;//发布此时的时间点
                _pub_gpos.get().lat = lat;   //发布纬度
                _pub_gpos.get().lon = lon;   //发布经度
                _pub_gpos.get().alt = alt;   //发布海拔高度
                _pub_gpos.get().vel_n = xLP(X_vx);   //发布向北的速度
                _pub_gpos.get().vel_e = xLP(X_vy);   //发布向东的速度
                _pub_gpos.get().vel_d = xLP(X_vz);   //发布向下的速度

                _pub_gpos.get().yaw = _eul(2);       //发布偏航角速度
                _pub_gpos.get().eph = eph;           //发布eph
                _pub_gpos.get().epv = epv;           //发布epv
                _pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter; //发布气压高度
                _pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);  //发布地面的海拔高度，注意：X_tz是地面到原点的距离
                _pub_gpos.get().terrain_alt_valid = _estimatorInitialized & EST_TZ;//发布地面的海拔高度信息的有效性
                _pub_gpos.get().dead_reckoning = !(_estimatorInitialized & EST_XY);//发布是不是用了dead_reckoninggps（惯性导航技术）
                _pub_gpos.update();
    }
}


/**初始化状态协方差矩阵P**/
void BlockLocalPositionEstimator::initP()
{
    _P.setZero();//将P清零
    // initialize to twice valid condition
    // 疑惑：为什么要初始化成两倍的方差？根据协方差矩阵的定义不应该就是方差吗？
    _P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;//=8
    _P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID * EST_STDDEV_XY_VALID;//=8
    _P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID * EST_STDDEV_Z_VALID;//=8
    _P(X_vx, X_vx) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();//=8
    _P(X_vy, X_vy) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();//=8
    // use vxy thresh for vz init as well
    _P(X_vz, X_vz) = 2 * _vxy_pub_thresh.get() * _vxy_pub_thresh.get();//=8
    // initialize bias uncertainty to small values to keep them stable
    _P(X_bx, X_bx) = 1e-6;
    _P(X_by, X_by) = 1e-6;
    _P(X_bz, X_bz) = 1e-6;
    _P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID * EST_STDDEV_TZ_VALID;//=8
}


/**初始化A、B、Q、R**/
void BlockLocalPositionEstimator::initSS()
{
    initP();

    // dynamics matrix
    _A.setZero();
    // derivative of position is velocity
        // 这里只设置了A的其中三个，其余的在下面的updateSSStates()函数中。
    _A(X_x, X_vx) = 1;
    _A(X_y, X_vy) = 1;
    _A(X_z, X_vz) = 1;

    // input matrix
    _B.setZero();
    _B(X_vx, U_ax) = 1;
    _B(X_vy, U_ay) = 1;
    _B(X_vz, U_az) = 1;

    // update components that depend on current state
    updateSSStates();
    updateSSParams();
}


/**设置A**/
void BlockLocalPositionEstimator::updateSSStates()
{
    // derivative of velocity is accelerometer acceleration
    // (in input matrix) - bias (in body frame)
    // 速度的微分就是加速度计的加速度信息减去偏差
    _A(X_vx, X_bx) = -_R_att(0, 0);
    _A(X_vx, X_by) = -_R_att(0, 1);
    _A(X_vx, X_bz) = -_R_att(0, 2);

    _A(X_vy, X_bx) = -_R_att(1, 0);
    _A(X_vy, X_by) = -_R_att(1, 1);
    _A(X_vy, X_bz) = -_R_att(1, 2);

    _A(X_vz, X_bx) = -_R_att(2, 0);
    _A(X_vz, X_by) = -_R_att(2, 1);
    _A(X_vz, X_bz) = -_R_att(2, 2);
}


/**设置R、Q**/
void BlockLocalPositionEstimator::updateSSParams()
{
    // input noise covariance matrix
    _R.setZero();
    _R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();//平方
    _R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();//平方
    _R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();  //平方

    // process noise power matrix
    _Q.setZero();
    float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
    float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
    _Q(X_x, X_x) = pn_p_sq;
    _Q(X_y, X_y) = pn_p_sq;
    _Q(X_z, X_z) = pn_p_sq;
    _Q(X_vx, X_vx) = pn_v_sq;
    _Q(X_vy, X_vy) = pn_v_sq;
    _Q(X_vz, X_vz) = pn_v_sq;

    // technically, the noise is in the body frame,
    // but the components are all the same, so
    // ignoring for now
    float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
    _Q(X_bx, X_bx) = pn_b_sq;
    _Q(X_by, X_by) = pn_b_sq;
    _Q(X_bz, X_bz) = pn_b_sq;

    // terrain random walk noise ((m/s)/sqrt(hz)), scales with velocity
    float pn_t_noise_density =
        _pn_t_noise_density.get() +
        (_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
    _Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}


/******预测*******/
void BlockLocalPositionEstimator::predict()
{

        // 获取加速度信息
        matrix::Quatf q(&_sub_att.get().q[0]);//括号里的q来自vehicle_attitude这个topic，也就是经过传感器数据融合后修正的q，这里为什么是q（0），其实这里的q(0)指的是首地址
        _eul = matrix::Euler<float>(q);  //由四元数得到飞机的姿态（欧拉角）
        _R_att = matrix::Dcm<float>(q);  //由四元数得到旋转矩阵
        Vector3f a(_sub_sensor.get().accelerometer_m_s2); //a向量里面是加速度信息
        // note, bias is removed in dynamics function
        // 准备输入向量_u
        _u = _R_att * a;    //转换到大地系
        _u(U_az) += 9.81f;  // add g  地理坐标系下的z轴加速度是有重力加速度的，因此补偿上去。



        // update state space based on new states
        // 更新系统状态空间转移矩阵，即A矩阵
        updateSSStates();






        // continuous time kalman filter prediction
        // integrate runge kutta 4th order
        // TODO move rk4 algorithm to matrixlib
        // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
        float h = getDt(); //起初看到这个h我先猜是教材中ekf公式中的dt，还是pid中的dt？后面发现都不是，而是指rk中的步长，这里指时间间隔
        Vector<float, n_x> k1, k2, k3, k4;  //从这里可知用到了四阶的龙格库塔，k1到k4表示取的中间四个点的斜率。


        k1 = dynamics(0, _x, _u);//dynamics就是现代控制理论中的动态方程，是一个一阶的微分方程
        k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
        k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
        k4 = dynamics(h, _x + k3 * h, _u);
        Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);//dx是下一时刻的状态的变化值，也就是博客里的deltaY

        // dx的限制性处理
        if (!(_estimatorInitialized & EST_XY))  //相与后非表示没有初始化或者xy没有有效数据
        {
                dx(X_x) = 0;   //x方向的位置变化为零
                dx(X_vx) = 0;  //x方向的速度变化为零
                dx(X_y) = 0;   //y方向的位置变化为零
                dx(X_vy) = 0;  //y方向的速度变化为零
        }



        // don't integrate z if no valid z data
        if (!(_estimatorInitialized & EST_Z))
        {
                dx(X_z) = 0;   //z方向的位置变化为零
        }



        // don't integrate tz if no valid tz data
        if (!(_estimatorInitialized & EST_TZ))
        {
                dx(X_tz) = 0;  //距离地面的高度变化为零
        }

        // saturate bias
        // 计算偏差
        float bx = dx(X_bx) + _x(X_bx);
        float by = dx(X_by) + _x(X_by);
        float bz = dx(X_bz) + _x(X_bz);

        //下面对偏差bx、by、bz进行判断，看是否大于规定的最大偏差如果大了就进行处理
        if (std::abs(bx) > BIAS_MAX)
        {
            bx = BIAS_MAX * bx / std::abs(bx);
            dx(X_bx) = bx - _x(X_bx);
        }

        if (std::abs(by) > BIAS_MAX)
        {
            by = BIAS_MAX * by / std::abs(by);
            dx(X_by) = by - _x(X_by);
        }

        if (std::abs(bz) > BIAS_MAX) 
        {
            bz = BIAS_MAX * bz / std::abs(bz);
            dx(X_bz) = bz - _x(X_bz);
        }


        _x += dx;// _x就是下一时刻的预测值，接下来的任务就是对他进行校正

        //下面是p的一阶微分方程，在解这个方程的时候，用的是最简单的欧拉法
        Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
                      _B * _R * _B.transpose() + _Q) * getDt();

        // dp的限制性处理
        // 处理的内容是：如果本身的p已经大于了P_MAX，则不会再累加dp
        for (int i = 0; i < n_x; i++)
        {
                if (_P(i, i) > P_MAX)
                {
                    // if diagonal element greater than max, stop propagating
                    dP(i, i) = 0;

                    for (int j = 0; j < n_x; j++)
                    {
                       dP(i, j) = 0;
                       dP(j, i) = 0;
                    }
                }
        }

        _P += dP;
        _xLowPass.update(_x);       //将_x低通处理后存在_xLowPass里的state中
        _aglLowPass.update(agl());  //将agl低通处理后存在_aglLowPass的state中
}



int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
{
    float t_delay = 0;
    uint8_t i_hist = 0;

    for (i_hist = 1; i_hist < HIST_LEN; i_hist++) {
        t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i_hist)(0, 0));

        if (t_delay > delay) {
            break;
        }
    }

    *periods = i_hist;

    if (t_delay > DELAY_MAX) {
        mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
        return -1;
    }

    return OK;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153

#pragma once

#include <px4_posix.h>
#include <px4_module_params.h>   //c++里的base class,用于其他的参数使用配置参数
#include <controllib/blocks.hpp> //之前同学讲过的
#include <mathlib/mathlib.h>    //数学库
#include <lib/ecl/geo/geo.h>
#include <matrix/Matrix.hpp>    //矩阵


// 下面就是本程序的订阅和发布的信息流。
// uORB Subscriptions  一些订阅的topics
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>   //飞机的状态
#include <uORB/topics/actuator_armed.h>   //执行机构的armed情况
#include <uORB/topics/vehicle_land_detected.h>  //检查是否着陆
#include <uORB/topics/vehicle_control_mode.h>   //控制模式
#include <uORB/topics/vehicle_attitude.h>       //飞机的姿态
#include <uORB/topics/vehicle_attitude_setpoint.h>  //设置的飞机的姿态
#include <uORB/topics/optical_flow.h>     //光流
#include <uORB/topics/sensor_combined.h>  //传感器数据
#include <uORB/topics/distance_sensor.h>  //距离传感器
#include <uORB/topics/parameter_update.h>   //参数更新
#include <uORB/topics/manual_control_setpoint.h>  //手动控制下的一些设定值
#include <uORB/topics/vehicle_gps_position.h>    //飞行器的gps位置
#include <uORB/topics/att_pos_mocap.h>     //通过mocap获取的姿态位置信息
#include <uORB/topics/landing_target_pose.h>  //着陆点的相对位置

// uORB Publications  发布的信息
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/estimator_status.h>  //估计器的状态，这里的估计器包括之前用于位置姿态估计用到的各种传感器以及其他模块
#include <uORB/topics/ekf2_innovations.h>

using namespace matrix;
using namespace control;

static const float DELAY_MAX = 0.5f;   // 延迟的最大值=0.5s
static const float HIST_STEP = 0.05f;  // 20 hz，历史消息间隔？
static const float BIAS_MAX = 1e-1f;   //偏差最大值
static const size_t HIST_LEN = 10;     // DELAY_MAX / HIST_STEP;历史消息的长度
static const size_t N_DIST_SUBS = 4;   //待查？


// 统计学中的卡方检验：卡方检验就是统计样本的实际观测值与理论推断值之间的偏离程度，
// 实际观测值与理论推断值之间的偏离程度就决定卡方值的大小，卡方值越大，越不符合；卡方值越小，偏差越小，越趋于符合，
// 若两个值完全相等时，卡方值就为0，表明理论值完全符合。
// for fault detection
// chi squared distribution, false alarm probability 0.0001
// see fault_table.py
// note skip 0 index so we can use degree of freedom as index
static const float BETA_TABLE[7] = {0,
                    8.82050518214,
                    12.094592431,
                    13.9876612368,
                    16.0875642296,
                    17.8797700658,
                    19.6465647819,
                   };

class BlockLocalPositionEstimator : public control::SuperBlock, public ModuleParams
{
// dynamics:
//
//  x(+) = A * x(-) + B * u(+) ，系统状态方程
//  y_i = C_i*x  ， 观测方程
//
// kalman filter
//
//  E[xx'] = P  估计量的误差的协方差
//  E[uu'] = W  系统噪声
//  E[y_iy_i'] = R_i  系统噪声协方差矩阵
//
//  prediction
//      x(+|-) = A*x(-|-) + B*u(+)
//      P(+|-) = A*P(-|-)*A' + B*W*B'  对应书上190页公式2
//
//  correction
//      x(+|+) =  x(+|-) + K_i * (y_i - H_i * x(+|-) )
//
//
// input:
//      ax, ay, az (acceleration NED)
//
// states:
//      px, py, pz , ( position NED, m)
//      vx, vy, vz ( vel NED, m/s),
//      bx, by, bz ( accel bias, m/s^2)
//      tz (terrain altitude, ASL, m)
//
// measurements:
//
//      sonar: pz (measured d*cos(phi)*cos(theta))
//
//      baro: pz
//
//      flow: vx, vy (flow is in body x, y frame)
//
//      gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
//      lidar: pz (actual measured d*cos(phi)*cos(theta))
//
//      vision: px, py, pz, vx, vy, vz
//
//      mocap: px, py, pz
//
//      land (detects when landed)): pz (always measures agl = 0)
//
public:

    // constants
    enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
    enum {U_ax = 0, U_ay, U_az, n_u};
    enum {Y_baro_z = 0, n_y_baro};
    enum {Y_lidar_z = 0, n_y_lidar};
    enum {Y_flow_vx = 0, Y_flow_vy, n_y_flow};
    enum {Y_sonar_z = 0, n_y_sonar};
    enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
    enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
    enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
    enum {Y_land_vx = 0, Y_land_vy, Y_land_agl, n_y_land};
    enum {Y_target_x = 0, Y_target_y, n_y_target};
    enum {POLL_FLOW = 0, POLL_SENSORS, POLL_PARAM, n_poll};
    enum {
        FUSE_GPS = 1 << 0,
        FUSE_FLOW = 1 << 1,
        FUSE_VIS_POS = 1 << 2,
        FUSE_LAND_TARGET = 1 << 3,
        FUSE_LAND = 1 << 4,
        FUSE_PUB_AGL_Z = 1 << 5,
        FUSE_FLOW_GYRO_COMP = 1 << 6,
        FUSE_BARO = 1 << 7
    };

    enum sensor_t {
        SENSOR_BARO = 1 << 0,
        SENSOR_GPS = 1 << 1,
        SENSOR_LIDAR = 1 << 2,
        SENSOR_FLOW = 1 << 3,
        SENSOR_SONAR = 1 << 4,
        SENSOR_VISION = 1 << 5,
        SENSOR_MOCAP = 1 << 6,
        SENSOR_LAND = 1 << 7,
        SENSOR_LAND_TARGET = 1 << 8,
    };

    enum estimate_t {
        EST_XY = 1 << 0,
        EST_Z = 1 << 1,
        EST_TZ = 1 << 2,
    };

    // public methods
        BlockLocalPositionEstimator();//BlockLocalPositionEstimator类的构造函数
        void update();    //作用：对飞机的状态先验估计值进行补偿校正
        virtual ~BlockLocalPositionEstimator() = default;  //析构函数

private:
    BlockLocalPositionEstimator(const BlockLocalPositionEstimator &) = delete;
    BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &) = delete;

    // methods
    // ----------------------------
    //
        Vector<float, n_x> dynamics  //动态方程，形式为：x‘ = _A * x + _B * u，这是一个一阶微分方程，也就是
                                     //描述系统状态空间的状态方程，属于现代控制理论中的。
                                     //区分kf中的 x_(k) = A*x(k-1) + B*u_(k-1)。
                                     //本程序中的状态估计用的是这个一阶微分方程结合龙哥库塔，而不是用的kf中的第一个方程。因为
                                     //这是一个连续系统
        (
                float t,
                const Vector<float, n_x> &x,
                const Vector<float, n_u> &u
        );

        void initP();   //初始化状态协方差矩阵P
        void initSS();  //这个函数包括了下面的两个函数，执行这个函数的同时也就执行了下面的两个
        void updateSSStates();  //设置A
        void updateSSParams();  //设置R、Q

        // 预测下一时刻的空间状态
        void predict();

        // lidar 雷达的相关函数
        int  lidarMeasure(Vector<float, n_y_lidar> &y);  //数据测量
        void lidarCorrect();     //将之前用predict()预测的状态结合雷达数据进行校正
        void lidarInit();        //雷达的初始化
        void lidarCheckTimeout();//检查超时吗

    // sonar
        int  sonarMeasure(Vector<float, n_y_sonar> &y);//数据测量
        void sonarCorrect();     //将之前用predict()预测的状态结合声呐数据进行校正
        void sonarInit();        //声呐的初始化
        void sonarCheckTimeout();//检查超时吗

    // baro
        int  baroMeasure(Vector<float, n_y_baro> &y);//数据测量
        void baroCorrect();      //将之前用predict()预测的状态结合气压计数据进行校正
        void baroInit();         //气压计的初始化
        void baroCheckTimeout(); //检查超时吗

    // gps
        int  gpsMeasure(Vector<double, n_y_gps> &y);//数据测量
        void gpsCorrect();       //将之前用predict()预测的状态结合gps数据进行校正
        void gpsInit();          //gps的初始化
        void gpsCheckTimeout();  //检查超时吗

    // flow
        int  flowMeasure(Vector<float, n_y_flow> &y);//数据测量
        void flowCorrect();      //将之前用predict()预测的状态结合光流数据进行校正
        void flowInit();         //光流的初始化
        void flowCheckTimeout(); //检查超时吗

    // vision
        int  visionMeasure(Vector<float, n_y_vision> &y);//数据测量
        void visionCorrect();    //将之前用predict()预测的状态结合视觉数据进行校正
        void visionInit();       //视觉的初始化
        void visionCheckTimeout(); //检查超时吗

    // mocap
        int  mocapMeasure(Vector<float, n_y_mocap> &y);//数据测量
        void mocapCorrect();      //将之前用predict()预测的状态结合mocap数据进行校正
        void mocapInit();         //mocap的初始化
        void mocapCheckTimeout(); //检查超时吗

    // land
        int  landMeasure(Vector<float, n_y_land> &y);//数据测量
        void landCorrect();      //将之前用predict()预测的状态结合land数据进行校正
        void landInit();         //land的初始化
        void landCheckTimeout(); //检查超时吗

    // landing target
        int  landingTargetMeasure(Vector<float, n_y_target> &y);//数据测量
        void landingTargetCorrect();    //将之前用predict()预测的状态结合landing target数据进行校正
        void landingTargetInit();       //landing target的初始化
        void landingTargetCheckTimeout();//检查超时吗

    // timeouts
        void checkTimeouts();           //检查超时

    // misc
        inline float agl()  //用于检测是否着陆，着陆时agl=0.
    {
                return _x(X_tz) - _x(X_z);// _x(X_tz是地面到原点的z， _x(X_z)是飞行器到原点的z
    }

    bool landed();
        int getDelayPeriods(float delay, uint8_t *periods); //获取延迟时间长度

        // publications 用于发布信息的函数
    void publishLocalPos();
    void publishGlobalPos();
    void publishEstimatorStatus();

    // attributes
    // ----------------------------

        // subscriptions
    uORB::Subscription<actuator_armed_s> _sub_armed;
    uORB::Subscription<vehicle_land_detected_s> _sub_land;
    uORB::Subscription<vehicle_attitude_s> _sub_att;
    uORB::Subscription<optical_flow_s> _sub_flow;
    uORB::Subscription<sensor_combined_s> _sub_sensor;
    uORB::Subscription<parameter_update_s> _sub_param_update;
    uORB::Subscription<vehicle_gps_position_s> _sub_gps;
    uORB::Subscription<vehicle_local_position_s> _sub_vision_pos;
    uORB::Subscription<att_pos_mocap_s> _sub_mocap;
    uORB::Subscription<distance_sensor_s> _sub_dist0;
    uORB::Subscription<distance_sensor_s> _sub_dist1;
    uORB::Subscription<distance_sensor_s> _sub_dist2;
    uORB::Subscription<distance_sensor_s> _sub_dist3;
    uORB::Subscription<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
    uORB::Subscription<distance_sensor_s> *_sub_lidar;
    uORB::Subscription<distance_sensor_s> *_sub_sonar;
    uORB::Subscription<landing_target_pose_s> _sub_landing_target_pose;

    // publications
    uORB::Publication<vehicle_local_position_s> _pub_lpos;
    uORB::Publication<vehicle_global_position_s> _pub_gpos;
    uORB::Publication<estimator_status_s> _pub_est_status;
    uORB::Publication<ekf2_innovations_s> _pub_innov;

    // map projection
    struct map_projection_reference_s _map_ref;     //地图构建的参考。这个结构体在基于gps的位置控制中有用到，
                                                    //用于map_projection_project函数和map_projection_global_reproject
                                                    //函数，前者是将地理学坐标系(geographic coordinate system)中的点(球)投影到本地方位等距平面(XOY)中
                                                    //后者是将本地方位等距平面中的点投影到地理学坐标系，也就是球面坐标和平面坐标之间的转换


    DEFINE_PARAMETERS(
        (ParamInt<px4::params::SYS_AUTOSTART>) _sys_autostart,   /**< example parameter */

        // general parameters
        (ParamInt<px4::params::LPE_FUSION>) _fusion,
        (ParamFloat<px4::params::LPE_VXY_PUB>) _vxy_pub_thresh,
        (ParamFloat<px4::params::LPE_Z_PUB>) _z_pub_thresh,

        // sonar parameters
        (ParamFloat<px4::params::LPE_SNR_Z>) _sonar_z_stddev,
        (ParamFloat<px4::params::LPE_SNR_OFF_Z>) _sonar_z_offset,

        // lidar parameters
        (ParamFloat<px4::params::LPE_LDR_Z>) _lidar_z_stddev,
        (ParamFloat<px4::params::LPE_LDR_OFF_Z>) _lidar_z_offset,

        // accel parameters
        (ParamFloat<px4::params::LPE_ACC_XY>) _accel_xy_stddev,
        (ParamFloat<px4::params::LPE_ACC_Z>) _accel_z_stddev,

        // baro parameters
        (ParamFloat<px4::params::LPE_BAR_Z>) _baro_stddev,

        // gps parameters
        (ParamFloat<px4::params::LPE_GPS_DELAY>) _gps_delay,
        (ParamFloat<px4::params::LPE_GPS_XY>) _gps_xy_stddev,
        (ParamFloat<px4::params::LPE_GPS_Z>) _gps_z_stddev,
        (ParamFloat<px4::params::LPE_GPS_VXY>) _gps_vxy_stddev,
        (ParamFloat<px4::params::LPE_GPS_VZ>) _gps_vz_stddev,
        (ParamFloat<px4::params::LPE_EPH_MAX>) _gps_eph_max,
        (ParamFloat<px4::params::LPE_EPV_MAX>) _gps_epv_max,

        // vision parameters
        (ParamFloat<px4::params::LPE_VIS_XY>) _vision_xy_stddev,
        (ParamFloat<px4::params::LPE_VIS_Z>) _vision_z_stddev,
        (ParamFloat<px4::params::LPE_VIS_DELAY>) _vision_delay,

        // mocap parameters
        (ParamFloat<px4::params::LPE_VIC_P>) _mocap_p_stddev,

        // flow parameters
        (ParamFloat<px4::params::LPE_FLW_OFF_Z>) _flow_z_offset,
        (ParamFloat<px4::params::LPE_FLW_SCALE>) _flow_scale,
        (ParamInt<px4::params::LPE_FLW_QMIN>) _flow_min_q,
        (ParamFloat<px4::params::LPE_FLW_R>) _flow_r,
        (ParamFloat<px4::params::LPE_FLW_RR>) _flow_rr,

        // land parameters
        (ParamFloat<px4::params::LPE_LAND_Z>) _land_z_stddev,
        (ParamFloat<px4::params::LPE_LAND_VXY>) _land_vxy_stddev,

        // process noise
        (ParamFloat<px4::params::LPE_PN_P>) _pn_p_noise_density,
        (ParamFloat<px4::params::LPE_PN_V>) _pn_v_noise_density,
        (ParamFloat<px4::params::LPE_PN_B>) _pn_b_noise_density,
        (ParamFloat<px4::params::LPE_PN_T>) _pn_t_noise_density,
        (ParamFloat<px4::params::LPE_T_MAX_GRADE>) _t_max_grade,

        (ParamFloat<px4::params::LPE_LT_COV>) _target_min_cov,
        (ParamInt<px4::params::LTEST_MODE>) _target_mode,

        // init origin
        (ParamInt<px4::params::LPE_FAKE_ORIGIN>) _fake_origin,
        (ParamFloat<px4::params::LPE_LAT>) _init_origin_lat,
        (ParamFloat<px4::params::LPE_LON>) _init_origin_lon
    )

    // target mode paramters from landing_target_estimator module
    // 与地面目标有关的参数，比如着陆点是固定还是移动的
    enum TargetMode {
                Target_Moving = 0, //目标移动状态
                Target_Stationary = 1  //目标为静止状态
    };

    // flow gyro filter 滤波器，这个在之前讲过的block类里面
    BlockHighPass _flow_gyro_x_high_pass;
    BlockHighPass _flow_gyro_y_high_pass;

    // stats 传感器或者其他模块的统计量
    BlockStats<float, n_y_baro> _baroStats;
    BlockStats<float, n_y_sonar> _sonarStats;
    BlockStats<float, n_y_lidar> _lidarStats;
    BlockStats<float, 1> _flowQStats;
    BlockStats<float, n_y_vision> _visionStats;
    BlockStats<float, n_y_mocap> _mocapStats;
    BlockStats<double, n_y_gps> _gpsStats;
    uint16_t _landCount;

    // low pass
    BlockLowPassVector<float, n_x> _xLowPass;
    BlockLowPass _aglLowPass;

    // delay blocks
    BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
    BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;

    // misc  一些时间变量
    px4_pollfd_struct_t _polls[3];
    uint64_t _timeStamp;
    uint64_t _time_origin;
    uint64_t _timeStampLastBaro;
    uint64_t _time_last_hist;
    uint64_t _time_last_flow;
    uint64_t _time_last_baro;
    uint64_t _time_last_gps;
    uint64_t _time_last_lidar;
    uint64_t _time_last_sonar;
    uint64_t _time_init_sonar;
    uint64_t _time_last_vision_p;
    uint64_t _time_last_mocap;
    uint64_t _time_last_land;
    uint64_t _time_last_target;

    // reference altitudes
    float _altOrigin;//原点的海拔
    bool _altOriginInitialized;//原点海拔初始化
    bool _altOriginGlobal; // true when the altitude of the origin is defined wrt a global reference frame
    float _baroAltOrigin;//原点的气压高度
    float _gpsAltOrigin;//原点的gps高度

    // status
    bool _receivedGps;
    bool _lastArmedState;

    // masks
    uint16_t _sensorTimeout; //传感器超时时间
    uint16_t _sensorFault; //传感器故障
    uint8_t _estimatorInitialized;

    // sensor update flags
    bool _flowUpdated;//光流传感器更新
    bool _gpsUpdated; //gps更新
    bool _visionUpdated;//视觉更新
    bool _mocapUpdated;//motion capture更新
    bool _lidarUpdated;//雷达更新
    bool _sonarUpdated;//声呐更新
    bool _landUpdated; //land detector更新
    bool _baroUpdated;//气压计更新

    // state space
    Vector<float, n_x>  _x; // 状态向量
    Vector<float, n_u>  _u; // 系统输入量
    Matrix<float, n_x, n_x>  _P;    // 状态协方差矩阵

    matrix::Dcm<float> _R_att;//旋转矩阵，为了与下面的输入协方差矩阵_R区别,于是加了_att
    Vector3f _eul;

    Matrix<float, n_x, n_x>  _A;    // dynamics matrix  动态矩阵，也叫系统矩阵
    Matrix<float, n_x, n_u>  _B;    // input matrix    输入矩阵
    Matrix<float, n_u, n_u>  _R;    // input covariance 输入的噪声协方差矩阵
    Matrix<float, n_x, n_x>  _Q;    // process noise covariance 过程噪声的协方差矩阵
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442