PX4-terrain_estimator

by luoshi006

参考：
https://github.com/PX4/Firmware/blob/master/src/lib/terrain_estimation/terrain_estimator.h


PX4 位置估计中的 TerrainEstimator，直译为地形估计，用于估计地形高度 。

该部分代码使用卡尔曼滤波进行估计，分为* 预测 - 校正 *两步。

---

变量

    // 函数主要变量。
    matrix::Vector<float, n_x> _x;      // state: ground distance, velocity, accel bias in z direction
    float  _u_z;            // acceleration in earth z direction
    matrix::Matrix<float, 3, 3> _P; // covariance matrix
    ....
    //构造函数中的初始化：
    _u_z = 0.0f;
    _P.setIdentity();//单位阵；

---

主要函数声明

//函数主要部分【预测】【校正】。
    void predict(float dt, const struct vehicle_attitude_s *attitude, 
                const struct sensor_combined_s *sensor,
                const struct distance_sensor_s *distance);

    void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
                const struct distance_sensor_s *distance,
                const struct vehicle_attitude_s *attitude);

---

预测 predict

void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude,
                   const struct sensor_combined_s *sensor,
                   const struct distance_sensor_s *distance)
{
    //获取当前姿态旋转矩阵;
    matrix::Dcmf R_att = matrix::Quatf(attitude->q);
    //加速度计读数；
    matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
    matrix::Vector<float, 3> u;
    //将加速度计读数投影到导航坐标系；
    u = R_att * a;
    //补偿重力加速度 9.80665f  m/s^2
    _u_z = u(2) + CONSTANTS_ONE_G; // compensate for gravity

    // dynamics matrix
    matrix::Matrix<float, n_x, n_x> A;
    A.setZero(); //        [ 0 1 0 ]
    A(0, 1) = 1; //        [ 0 0 1 ]
    A(1, 2) = 1; //        [ 0 0 0 ]  

    // input matrix
    matrix::Matrix<float, n_x, 1>  B;
    B.setZero();  //        [ 0 ]
    B(1, 0) = 1;  //        [ 1 ]
                  //        [ 0 ]

    // input noise variance
    // 博主认为此处 R 为加速度计参数
    float R = 0.135f;

    // process noise convariance
    //这里的Q为零阵，好像没有用，作用被上方的R代替；
    matrix::Matrix<float, n_x, n_x>  Q;
    Q(0, 0) = 0;
    Q(1, 1) = 0;

    // do prediction
    //此处代码解释见下方图片内容；
    matrix::Vector<float, n_x>  dx = (A * _x) * dt;
    dx(1) += B(1, 0) * _u_z * dt;

    // propagate state and covariance matrix
    _x += dx;
    _P += (A * _P + _P * A.transpose() +
           B * R * B.transpose() + Q) * dt;
}

---

校正 update

此处 distance_sensor_s 对应的传感器有[laser range finder]、[ultra Sound]、[infrared]。

void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
        const struct distance_sensor_s *distance,
        const struct vehicle_attitude_s *attitude)
{
    // terrain estimate is invalid if we have range sensor timeout
    if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) {
        _terrain_valid = false;
    }

    if (distance->timestamp > _time_last_distance) {
        matrix::Quatf q(attitude->q);
        matrix::Eulerf euler(q);
        //传感器测量高度；
        float d = distance->current_distance;

        matrix::Matrix<float, 1, n_x> C;
        C(0, 0) = -1; // measured altitude,  [ -1 0 0 ]

        float R = 0.009f; //并不知道怎么得到的这个参数；

        matrix::Vector<float, 1> y;
        //校正 roll  pitch 引起的高度变化；
        y(0) = d * cosf(euler.phi()) * cosf(euler.theta());

        // residual 残差
        //此处代码解释见下方图片内容；
        matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
        S_I(0, 0) += R;
        S_I = matrix::inv<float, 1> (S_I);
        matrix::Vector<float, 1> r = y - C * _x;

        matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;

        // some sort of outlayer rejection 野值处理
        if (fabsf(distance->current_distance - _distance_last) < 1.0f) {
            _x += K * r;
            _P -= K * C * _P;
        }

        ....

        }

GPS 部分代码，原理同上，用于校正 velocity。
gps->vel_d_m_s GPS Down Velocity (m/s) 。

---