写在前面
源码版本:1.6.0rc1
源码位置:Firmware-1.6.0rc1\src\modules\position_estimator_inav\position_estimator_inav_main.cpp
整体框架:
整个算法的核心思想是由地理坐标系下的加速度通过积分,来获得速度、位置信息;经过2次修正产生可利用的信息,第一次是利用传感器计算修正系数产生加速度的偏差修正加速度,第二次是利用修正系数修正位置;最后可利用速度经过加速度修正,可利用的位置经过了加速度和位置修正。加速度的修正过程是由机体测量的加速度通过减去偏差,再转换到地理坐标系;位置的修正统一利用inertial_filter_correct()函数。
滤波两个函数
函数位置:Firmware-1.6.0rc1\src\modules\position_estimator_inav\inertial_filter.cpp
//x[0]: 位置
//x[1]:速度
void inertial_filter_predict(float dt, float x[2], float acc)
{
if (PX4_ISFINITE(dt)) {
if (!PX4_ISFINITE(acc)) {
acc = 0.0f;
}
x[0] += x[1] * dt + acc * dt * dt / 2.0f;
x[1] += acc * dt;
}
}
//e : 每个传感器所代表的动态误差(corr 型)
//i : i=0 弥补位置 i=1 弥补速度
//w : 这个传感器弥补所占的权重
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) {
//ki * e *dt 相当于积分环节 用于消除静态误差
float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) {
x[1] += w * ewdt;
}
}
}
第一个函数很好理解,就是利用物理公式 Pk = Pk-1 +Vkdt +1/2adt^2 Vk = Vk-1 + adt 得到预测的位置和速度。
第二个函数可以把它理解成一个积分环节,而积分环节作用是消除静态误差,e表示每个传感器的动态误差(corr型),i = 0 弥补位置,i = 1 弥补速度, w则是某个传感器弥补时候所占用的权重(因为不同传感器精度不同,所以所占权重也不同)。
PX4位置估计源码分析
由于代码篇幅过长,且是多个相同功能的传感器写在了同一个函数中,而我们只关注气压传感器、GPS传感器用于修正加速度计,其他传感器用的较少,所以雷达、视觉、mocap、传感器的源代码部分就不在做粘贴分析。
从功能函数入口开始 int position_estimator_inav_thread_main(int argc, char *argv[])
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 //估计值estimate
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
float z_est[2] = { 0.0f, 0.0f }; // pos, vel
float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer
float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer
float R_gps[3][3]; // rotation matrix for GPS correction moment
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;
static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation
float eph = max_eph_epv;
float epv = 1.0f;//水平因子越小代表获得的数据准确度越高
float 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
hrt_abstime accel_timestamp = 0;
hrt_abstime baro_timestamp = 0;
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_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
float corr_gps[3][2] = {//动态误差correction
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float w_gps_xy = 1.0f;//动态调节参数
float w_gps_z = 1.0f;
开头定义了很多变量,用于之后的计算,变量意义注释都有,没啥好说的。
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t vehicle_global_position_pub = NULL;
struct position_estimator_inav_params params;
memset(¶ms, 0, sizeof(params));
struct position_estimator_inav_param_handles pos_inav_param_handles;
/* initialize parameter handles */
inav_parameters_init(&pos_inav_param_handles);
/* first parameters read at start up */
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub,
¶m_update); /* read from param topic to clear updated flag */
/* first parameters update */
inav_parameters_update(&pos_inav_param_handles, ¶ms);
订阅及公告一些数据,参数初始化及参数更新。
px4_pollfd_struct_t fds_init[1] = {};
fds_init[0].fd = sensor_combined_sub;
fds_init[0].events = POLLIN;
定义阻塞等待某个消息,消息名称为:sensor_combined_sub。
/* 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) {
int ret = px4_poll(&fds_init[0], 1, 1000);
if (ret < 0) {
/* poll error */
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
wait_baro = false;
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) {
if (PX4_ISFINITE(sensor.baro_alt_meter)) {
baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
}
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
}
}
} else {
PX4_WARN("INAV poll timeout");
}
}//只是上电运行一次
阻塞等待消息句柄为:sensor_combined_sub的数据,获取气压值,200次求平均值。(注意:这个循环上电只会循环一次,循环结束会将wait_baro这个bool变量置为false)
下面将进入功能函数的主循环。
/* main loop */
px4_pollfd_struct_t fds[1];
fds[0].fd = vehicle_attitude_sub;
fds[0].events = POLLIN;
while (!thread_should_exit) {
int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
/* poll error */
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
continue;
} else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */
//参数更新
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
inav_parameters_update(&pos_inav_param_handles, ¶ms);
}
/* actuator */
orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* 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);
matrix::Dcmf R = matrix::Quatf(att.q);//姿态矩阵R
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
/* correct accel bias */
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] += R(i, j) * sensor.accelerometer_m_s2[j];
//acc 表示地理坐标系下准确加速度
}
}
acc[2] += CONSTANTS_ONE_G;//z轴弥补1G重力加速度
accel_timestamp = sensor.timestamp + sensor.accelerometer_timestamp_relative;
accel_updates++;
}
if (sensor.timestamp + sensor.baro_timestamp_relative != baro_timestamp) {
//高度差(气压计动态误差 corr型) = 起飞点高度 - 气压计当前高度 - z轴估计高度(负)
//baro_offset:气压计上电初值,即起飞前气压
//sensor.baro_alt_meter:气压计测得的当前气压高度
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_timestamp = sensor.timestamp + sensor.baro_timestamp_relative;
baro_updates++;
}
}
代码开头又定义了一个需要阻塞等待的消息,其消息句柄为:vehicle_attitude_sub。这段代码的作用就是开篇给出的整体框架中的一部分(如下图),其中 加速度计获得的是机体坐标系下的加速度值,加速度的偏差(acc_bias)也要是机体系下的偏差,而后面用于积分的真实加速度(acc)是地理系下的加速度,所以要乘以旋转矩阵。
这里先提出一个问题:acc_bias[x] 从哪里获得?
每个传感器都有两种误差 : 静态误差 动态误差(corr) 其中静态误差可以通过静止之后取平均值得到并储存,但是动态误差需要事实计算得到。
在上面这段代码最后,通过 高度差(气压计动态误差 corr型) = 起飞点高度 - 气压计当前高度 - z轴估计高度(负) 得到气压计 的corr(动态误差),下面代码也是获取其他传感器(雷达、视觉、光流、mocap、GPS)的corr,这里只粘贴出GPS如何获取的corr。
/* 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) {
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");
}
}
上面部分是检测GPS数据是否有更新,并且每次使用GPS数据前都要判断GPS数据是否有效,(在上一次有效的时候判断本次是否无效,在上一次无效的时候判断本次是否有效),判断标准:GPS的定位因子是否在合理范围内,GPS提供的定位卫星是否>=3。
if (gps_valid) {
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) {//初始化GPS 数据
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 */
map_projection_init(&ref, lat, lon);
// XXX replace this print
warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
}
}
这一部分上电只会运行一次,通过ref_inited变量判断GPS是否初始化,初始化完成后会将其赋值true。
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]));
/* reset position estimate when GPS becomes good */
if (reset_est) {
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 */
//位置corr计算 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 */
//速度corr计算 GPS值 - 估计值
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));
//计算动态权重,分母部分min_eph_epv = 2,gps.eph:水平定位因子、fmaxf(x1,x2)取两着最大
//定位因子越小,定位精度越高,当gps.eph > min_eph_epv 相当于放大分母,使得整体权重缩小
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
}//end if ref_inited.
} //end if gps_valid
else {
/* no GPS lock */
memset(corr_gps, 0, sizeof(corr_gps));
ref_init_start = 0;
}
gps_updates++;
}
}
w_gps_xy:GPS水平定位权重,w_gps_z:GPS垂直定位权重(看注释)
因为GPS有时延,所以计算的并不是本时刻的corr。
est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
这样一句话是将时间戳向前移步。
//当程序走到这里时候已经获取所有传感器的corr型
//下面就是这个corr型怎么使用
matrix::Dcm<float> R = matrix::Quatf(att.q);//得到旋转矩阵
//先判断上面计算的corr型是否在合理范围内
/* 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");
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms
t_prev = t;
/* increase EPH/EPV on each step */
if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
eph = 0.001;
}
if (eph < max_eph_epv) {
eph *= 1.0f + dt;
}
if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
epv = 0.001;
}
if (epv < max_eph_epv) {
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
//判断使用哪一个传感器(这里因为篇幅限制,只粘贴出GPS的定义变量)
/* 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;
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
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;
上面这段代码,首先通过姿态数据中定义旋转矩阵,判断上面计算的GPS的corr是否有效,(判断标准,GPS是否有效,GPS数据获取是否超时),并改变每次获取GPS之后的定位因子精度(我的理解:认为连续获取的GPS数据,越往后面的数据准确度越低),其次,定义一些使用GPS的标志位(需满足1. GPS已经初始化、2.GPS数据有效、3. GPS权重大于最小值。),最后计算动态权重(其中包含两部分 1.h文件中自带的权重 这些是定值、2.刚刚计算的动态权重)。
/* baro offset correction */
if (use_gps_z) {
float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
baro_offset += offs_corr;
corr_baro += offs_corr;
}
/* accelerometer bias correction for GPS (use buffered rotation matrix) */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps_xy) {//地理坐标系下的
accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
}
if (use_gps_z) {//我认为这里可能不用gps的z轴去计算加速度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;
}
// 这一部分可以看到z轴用气压计去融合的加速度计 而上面用的是GPS的z轴融合的加速度计
// 在代码中没有雷达就会用气压计去融合加速度计z轴数据
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 += R_gps[j][i] * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;//用于下一次 acc 矫正
}
}
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est, acc[2]);
/* inertial filter correction for altitude */
if (use_lidar) {
inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);
} else {//这里同样我认为会使用气压计去校正z轴,而不是用下面的GPS
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);
}
inav里面高度估计算法的理解:主要是用 acc.z 二次积分得到高度,但是 mpu6000 直接得到的 acc.z 并不能直接使用,所以用气压计算出一个矫正系数用来矫正 acc.z,然后二次积分得到高度,然后用气压计得到的高度直接矫正高度。也就是 说里面有 2 次矫正。这里得到的acc_bias[ ]用于下一次加速度计测量值减去的偏差(校正加速度)。
下面校正xy轴。
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));
}
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);
}
}
else {
/* gradually reset xy velocity estimates */
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);
}
先预测,后校正的形式,预测函数与校正函数就是前文提到的两个函数。
在后面是一个冗余切换的保护机制这里没有粘贴。
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], &R._data[0][0], sizeof(R._data));
buf_ptr++;
if (buf_ptr >= EST_BUF_SIZE) {
buf_ptr = 0;
}
/* publish local position */
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];
matrix::Eulerf euler(R);
local_pos.yaw = euler.psi();
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];
}
local_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
if (local_pos.xy_global && local_pos.z_global) {
/* publish global position */
global_pos.timestamp = t;
global_pos.time_utc_usec = gps.time_utc_usec;
double est_lat, est_lon;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
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);
}
}
}
}
数据填充、发布
总结:
- 变量初始化
- 计算气压计高度的零点偏移,主要是取 200 个数据求平均
- 各传感器计算得带各自的修正系数和权重
- 判断是否超时
- 判断是用哪一个传感器
- 计算权重
- 根据使用的传感器计算加速度偏差
- 预计位置
- 修正位置
- 发布
以上是个人对PX4位置估计的源码理解,同时也参考了阿木实验室better的理解思路,如有不正确的地方感谢提出批评指正,欢迎一起讨论学习QQ:1103706199。