本帖最后由 wilsonleong 于 2020-8-11 21:05 编辑
[quote]https://bbs.amovlab.com/forum.php?mod=redirect&goto=findpost&ptid=1324&pid=5305[/quote]
EKF2中update和predict是跑在两个不同的频率上的，update的频率是之前提过的IMU下采样的频率，而predict是IMU的硬件频率（也可能是Pixhawk的硬件频率）。再看飞机的状态：姿态的更新是跑在predict的频率下，只要有新的IMU数据，就会更新姿态并输出；位置和速度是跑在update的频率下的，也就是下采样的IMU频率，完成了一次EKF2的更新才会输出一次位置和速度；角速度可以直接从陀螺仪获取。
看代码：
ekf.cpp
bool Ekf::update()
{
bool updated = false;
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
// perform state and covariance prediction for the main filter
predictState();
predictCovariance();
// control fusion of observation data
controlFusionModes();
// run a separate filter for terrain estimation
runTerrainEstimator();
updated = true;
// run EKF-GSF yaw estimator
runYawEKFGSF();
}
// the output observer always runs
// Use full rate IMU data at the current time horizon
calculateOutputStates();
return updated;
}
ekf2_main.cpp（省略不少）
void Ekf2::Run()
{
sensor_combined_s sensors;
if (_sensors_sub.update(&sensors)) {
_ekf.setIMUData(imu_sample_new);
// publish attitude immediately (uses quaternion from output predictor)
publish_attitude(sensors, now);
// run the EKF update and output
perf_begin(_ekf_update_perf);
const bool updated = _ekf.update();
perf_end(_ekf_update_perf);
if (updated) {
// only publish position after successful alignment
if (control_status.flags.tilt_align) {
// publish vehicle local position data
_vehicle_local_position_pub.update();
// publish vehicle odometry data
_vehicle_odometry_pub.publish(odom);
}
}
// publish ekf2_timestamps
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
}
| 
发表于 2020-8-11 22:25:39
