|
楼主 |
发表于 2017-6-19 10:19:28
|
显示全部楼层
本帖最后由 joyrus 于 2017-6-19 14:09 编辑
续...
这回就要说位置互补算法了,还是先上代码,慢慢看。
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est, acc[0]); //x_est[1]=vel, [0]=pos, pitch
inertial_filter_predict(dt, y_est, acc[1]); //y_est[1]=vel, [0]=pos, roll
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && 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);
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_velocity + 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 (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && 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);
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_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 */
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);
}
就这么简单!
inertial_filter_predict(dt, x_est, acc[0]); //x_est[1]=vel, [0]=pos, pitch
inertial_filter_predict(dt, y_est, acc[1]); //y_est[1]=vel, [0]=pos, roll
void inertial_filter_predict(float dt, float x[2], float acc)
{
if (isfinite(dt)) {
if (!isfinite(acc)) {
acc = 0.0f;
}
x[0] += x[1] * dt + acc * dt * dt / 2.0f;//位置
x[1] += acc * dt; //速度
}
}
首先用加速度计的数据预测无人机的速度与位置,这里的加速度数据是已经经过误差补偿的结果。计算过程简单明了,就是物理知识。
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);
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);
上面是位置互补,下面是速度互补,看一下函数原型:
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
if (isfinite(e) && isfinite(w) && isfinite(dt)) {
float ewdt = e * w * dt;
x += ewdt;
if (i == 0) {
x[1] += w * ewdt;
}
}
}
e就是corr_gps的补偿量,这里实际上就是位置的误差,ewdt 就是依据误差与时间和权重的计算结果,用于对估算结果的补偿。
好了完成了!第一阶段的初步流程学习笔记已经整理完成,接下来准备第二阶段的部分细节的展开,需要些时间来消耗,大家见谅!
欢迎和我一起讨论,QQ15953321
|
|