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)
。