加速度计和气压计、激光互补滤波融合算法
融合原理：
- 加速度积分出来【速度】和【位移】
- 由于积分会累计误差，倒置积分结果越偏越大
- 用气压计来修正积分产生的误差
- 修正分成位移修正和速度修正，两个都需要修正，否则偏离都会越来越大
- 修正方法：第二传感器结果【减去】第一传感器结果，等到【误差】
- 【误差】乘以一个【系数】得到【修正值】
- 第一传感器加上【修正值】即得到【融合结果】
- 【系数】一般需要乘以dt，不乘也行，你看这个系数是不是和阶低通的系数很像，它们效果也很像：越小滤波越强，变化越小，数据越平缓，反应也越慢
- 网上大多算法只用了气压计的高度修正，但速度没有修正，如果用这个速度去定高，这个速度肯定会炸的
- 用气压计高度的微分去修正速度，权重可比高度大一点，保证速度不会炸，这时可以把if(i==0)那部分去掉
- 如果没有速度修正，也可心用修正后的高度微分+低通来算一个速度去定高，但反应会很慢
- 融合后的位移也是没法完成避免气压计的缓慢飘动的，但这个飘动很小
- 无人机的快速上下刹停还是要靠速度
- 想要高度更稳就要加入激光，修正方法同理，权重可以超级加倍，只修正高度就可以了
INV算法：
/** 函数名：inertial_filter_predict* 功能：一次预测*/
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* 功能：一次校正*/
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[i] += ewdt;if (i == 0){x[1] += w * w * ewdt;}}
}
用法：
void estimator_update(float dt)
{if (dt < 0.01f)dt = 0.01f; // 防止dt过小if (!FC_STATE_CHECK(FC_ARMED)){// 未解锁时，加大baro权重, 防止acc积分误差过大baro_auto_gain = 10.f;}else{if (!FC_STATE_CHECK(FC_AIRBORN)) // 起飞时权重小，避免地面效应{baro_auto_gain = 0.1f;}else{if (baro_auto_gain < 1.0f)//恢复到正常值{baro_auto_gain += 0.01f;}else{baro_auto_gain = 1.0f;}}}if (FC_STATE_CHECK(FC_IMU_CALIBRATING) || (millis() < 500)){/*校准时重置高度、速度、气压起始高度 */fc_sensor_data.baro.offset = fc_sensor_data.baro.altitude;tofAlt = 0;accel_bias_corr[Z] = 0;z_est[ALT_POS] = 0;z_est[ALT_VEL] = 0;}baroAlt = fc_sensor_data.baro.altitude - fc_sensor_data.baro.offset; // 气压高度（减去起点） 单位：cm，baro_vel = pt1FilterApply(&baro_v_fiter, (baroAlt - baroAlt_pre) / dt); // 气压速度(低通滤波，其实互补滤波本身就有低通效果，不用也行) 单位：cm/sbaro_vel = constrainf(baro_vel, -200.0f, 200.0f);//限制速度过大baroAlt_pre = baroAlt;acc[Z] = (zdrone_state.acc.z + accel_bias_corr[Z]) * GRAVITY_CMSS;//地理加速度转换成cm/s^2， 不是机体加速度acc_lpf[Z] = pt1FilterApply(&accZ_fiter, acc[Z]);//加速度低通滤波，其实互补滤波本身就有低通效果，不用也行if (acc_lpf[Z] > ACC_DEADBAND || acc_lpf[Z] < -ACC_DEADBAND){// 加死区}inertial_filter_predict(dt, z_est, acc[Z]);//加速度积分，得到速度和位置corr_baro = baroAlt - z_est[ALT_POS];//气压计误差corr_baro_vel = baro_vel - z_est[ALT_VEL];//速度误差inertial_filter_correct(corr_baro, dt, z_est, ALT_POS, est_params_f.w_baro_p * baro_auto_gain);//高度修正inertial_filter_correct(corr_baro_vel, dt, z_est, ALT_VEL, est_params_f.w_baro_v * baro_auto_gain);//速度修正accel_bias_corr[Z] += corr_baro_vel * est_params_f.w_baro_a * dt * dt * dt;//加速度修正isTofAltValid = fc_sensor_data.tof.range < 6000 && fc_sensor_data.tof.quality > 0 && fc_sensor_data.tof.isOnline;//激光距离有效if (isTofAltValid){//激光距离有效int16_t dtTof = fc_sensor_data.tof.range - tofRange_pre;tofRange_pre = fc_sensor_data.tof.range;if (ABS(dtTof) < 50)//丢弃激光距离变化过大的情况{/**激光变化量积分 */tofAlt += (dtTof * 0.1f); // cm}corr_tof = tofAlt - z_est[ALT_POS];//激光高度误差tof_vel = (tofAlt - tofAlt_pre) / dt;//激光速度tofAlt_pre = tofAlt;corr_tof_vel = tof_vel - z_est[ALT_VEL];inertial_filter_correct(corr_tof, dt, z_est, ALT_POS, est_params_f.w_tof_p * USED_TOF);//激光高度修正inertial_filter_correct(corr_tof_vel, dt, z_est, ALT_VEL, est_params_f.w_tof_v * USED_TOF);//激光速度修正accel_bias_corr[Z] += corr_tof_vel * est_params_f.w_tof_a * dt * dt * dt;obs_height = fc_sensor_data.tof.range * 0.1f;//光流对面的观测高度}else{tofAlt = z_est[ALT_POS];obs_height = z_est[ALT_POS];}obs_height = constrainf(obs_height, 1, 500);obs_height_lpf += (obs_height - obs_height_lpf) * 0.1f;estimator_xy_update(dt);z_est[ALT_VEL] = constrainf(z_est[ALT_VEL], -200.0f, 200.0f);//限制速度过大zdrone_state.pos.z = z_est[ALT_POS];//融合高度zdrone_state.vel.z = applyDeadbandFloat(z_est[ALT_VEL], 1);//融合速度
}