摸着石头过河,一边看一边学。
感谢两位博主的文章:
1、px4位置估计-inav
2、根据两点的经纬度求方位角和距离
主要过程如下
1、机体加速度转换
acc_bias为机体坐标系下加速度计的偏移量,初始值为0,imu_DCM为Body2NED转换矩阵,ori_xacc 、ori_yacc 、ori_zacc 为加速度计的原始值。将偏移修正后的加速度转到NED坐标系下,注意所使用的加速度计数据方向与标准方向需一致。
b_acc[0] = ori_xacc - acc_bias[0];
b_acc[1] = ori_yacc - acc_bias[1];
b_acc[2] = ori_zacc - acc_bias[2];
for (int i = 0; i < 3; i++) {
ned_acc[i] = 0.0;
for (int j = 0; j < 3; j++) {
ned_acc[i] += imu_DCM[j][i] * b_acc[j];
}
}
ned_acc[2] += 9.8;
2、GPS测量值与推测值误差
函数(map_projection_project)用于计算两经纬度之间的N,E方向的球面距离,单位(m)。ref 为收到GPS信号那一刻的经纬度,为参考位置零点。measure_lat,measure_lon为 GPS 时时的测量值。
float Dis_NE[2]; // 参考位置零点距离[北,东](m)
map_projection_project(&ref, measure_lat, measure_lon, &Dis_NE[0], &Dis_NE[1]);
est_buf 数组用于存放预测结果的缓冲数列,该数列的默认值均为0,求出GPS位置测量值Dis_NE与位置预测值、测量速度(measure_vn 、measure_ve)与速度预测值的误差。如果有需要可对校正值作数据跳变处理,对误差值进行阈值判断即可。新定义序列 (est_i) 与 (buf_i) 区分,使用缓冲数列里边的不同序列的数组用于处理GPS数据延迟。
int est_i = buf_i - 1 - MIN(EST_BUF_SIZE - 1, MAX(0, (int)(GPS_DELAY / dt)));
if(est_i < 0) est_i += EST_BUF_SIZE;
/*
定义 EST_BUF_SIZE = 25
est_buf[EST_BUF_SIZE][3][2] = {
{ 0.0, 0.0 }, // N (pos, vel)
{ 0.0, 0.0 }, // E (pos, vel)
{ 0.0, 0.0 }, // D (pos, vel)
}
corr_gps[3][2] = {
{ 0.0, 0.0 }, // N (pos, vel)
{ 0.0, 0.0 }, // E (pos, vel)
{ 0.0, 0.0 }, // D (pos, vel)
}
*/
corr_gps[0][0] = Dis_NE[0] - est_buf[est_i][0][0]; // N_Pos_Error
corr_gps[1][0] = Dis_NE[1] - est_buf[est_i][1][0]; // E_Pos_Error
corr_gps[0][1] = measure_vn - est_buf[est_i][0][1]; // N_Vel_Error
corr_gps[1][1] = measure_ve - est_buf[est_i][1][1]; // E_Vel_Error
corr_gps[2][1] = 0.0;
3、通过测量值与推测值的误差求NED坐标系下加速度的偏移ned_acc_bias_corr
选取合适的权值w_p、w_v,可得出ned_acc_bias_corr。
float ned_acc_bias_corr[3] = { 0.0, 0.0, 0.0 };
ned_acc_bias_corr[0] -= corr_gps[0][0] * w_p + corr_gps[0][1] * w_v;
ned_acc_bias_corr[1] -= corr_gps[1][0] * w_p + corr_gps[1][1] * w_v;
4、将 ned_acc_bias_corr转为机体坐标系补偿到 acc_bias
R_gps 为 NED2Body 转换矩阵,params_w_acc_bias 为 bias 补偿的权值
for (int i = 0; i < 3; i++) {
float c = 0.0;
for (int j = 0; j < 3; j++) {
c += R_gps[i][j] * ned_acc_bias_corr[j];
}
acc_bias[i] += c * params_w_acc_bias * dt;
}
这一步得出的 acc_bias 用于下一个周期的计算。
5、预测
(inertial_filter_predict) 为预测函数。其实也就是物理模型的表达式。
这一步其实就类似于卡尔曼滤波的 X = AX + Bu:
u = ned_acc;
static float N_Est[2] = {Dis_NE[0] , measure_vn};
static float E_Est[2] = {Dis_NE[1] , measure_ve};
inertial_filter_predict(dt, N_Est, ned_acc[0]);
inertial_filter_predict(dt, E_Est, ned_acc[1]);
/*
static 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.0; // dis = vt + 0.5*a*t^2
x[1] += acc * dt; // vel = a*t
}
}
*/
6、校正
通过实际值与预测值之间的误差对预测值进行调整。
这过程就类似于卡尔曼滤波中的 X = X_pre + K * (Z - HX_pre) ,H为单位阵,K为权值。
你品,你细细品…
inertial_filter_correct(corr_gps[0][0], dt, N_Est, w_p);
inertial_filter_correct(corr_gps[1][0], dt, E_Est, w_p);
inertial_filter_correct(corr_gps[0][1], dt, N_Est, w_v);
inertial_filter_correct(corr_gps[1][1], dt, E_Est, w_v);
/*
static void inertial_filter_correct(float e, float dt, float x[2], float w)
{
if (isfinite(e) && isfinite(w) && isfinite(dt))
{
float ewdt = e * w * dt;
x[i] += ewdt; // 积分对干扰信号有滤除作用
}
}
*/
7、保存结果
(map_projection_reproject)函数为已知一点经纬度以及 NE 方向距离求另一点经纬度的函数.
est_buf[buf_i][0][0] = N_Est[0];
est_buf[buf_i][0][1] = N_Est[1];
est_buf[buf_i][1][0] = E_Est[0];
est_buf[buf_i][1][1] = E_Est[1];
buf_i++;
buf_i = buf_i >= EST_BUF_SIZE ? 0 : buf_i;
Result->pre_vn = Result->vn;
Result->pre_ve = Result->ve;
Result->n = N_Est[0];
Result->vn = N_Est[1];
Result->e = E_Est[0];
Result->ve = E_Est[1];
// 将NED位置数据转换到wsg184坐标系
// Result->est_lat\est_lon 初始值为传感器测量值
map_projection_reproject(&ref, Result->n, Result->e, &Result->est_lat, &Result->est_lon);
// 计算NED加速度,速度控制使用
float n_acc = (Result->vn - Result->pre_vn)/dt;
float e_acc = (Result->ve - Result->pre_ve)/dt;
Result->n_acc = Result->nacc*(1 - 0.381) + n_acc *0.381;
Result->e_acc = Result->eacc*(1 - 0.381) + e_acc *0.381;
仅作学习理解参考,实际使用还需要考虑GPS丢星等情况。