摸着石头过河，一边看一边学。

感谢两位博主的文章：
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丢星等情况。