diff --git a/Tools/Replay/DataFlashFileReader.cpp b/Tools/Replay/DataFlashFileReader.cpp
index 3ab73e5ef8..207e127368 100644
--- a/Tools/Replay/DataFlashFileReader.cpp
+++ b/Tools/Replay/DataFlashFileReader.cpp
@@ -61,7 +61,7 @@ void AP_LoggerFileReader::get_packet_counts(uint64_t dest[])
     memcpy(dest, packet_counts, sizeof(packet_counts));
 }
 
-bool AP_LoggerFileReader::update(char type[5], uint8_t &core)
+bool AP_LoggerFileReader::update()
 {
     uint8_t hdr[3];
     if (read_input(hdr, 3) != 3) {
@@ -81,8 +81,6 @@ bool AP_LoggerFileReader::update(char type[5], uint8_t &core)
             return false;
         }
         memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
-        strncpy(type, "FMT", 3);
-        type[3] = 0;
 
         message_count++;
         return handle_log_format_msg(f);
@@ -103,9 +101,6 @@ bool AP_LoggerFileReader::update(char type[5], uint8_t &core)
         return false;
     }
 
-    strncpy(type, f.name, 4);
-    type[4] = 0;
-
     message_count++;
-    return handle_msg(f, msg, core);
+    return handle_msg(f, msg);
 }
diff --git a/Tools/Replay/DataFlashFileReader.h b/Tools/Replay/DataFlashFileReader.h
index 7d70ab8363..b03e077ac4 100644
--- a/Tools/Replay/DataFlashFileReader.h
+++ b/Tools/Replay/DataFlashFileReader.h
@@ -12,10 +12,10 @@ public:
     ~AP_LoggerFileReader();
 
     bool open_log(const char *logfile);
-    bool update(char type[5], uint8_t &core);
+    bool update();
 
     virtual bool handle_log_format_msg(const struct log_Format &f) = 0;
-    virtual bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) = 0;
+    virtual bool handle_msg(const struct log_Format &f, uint8_t *msg) = 0;
 
     void format_type(uint16_t type, char dest[5]);
     void get_packet_counts(uint64_t dest[]);
diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp
index 5e88977203..ef907ef67a 100644
--- a/Tools/Replay/LR_MsgHandler.cpp
+++ b/Tools/Replay/LR_MsgHandler.cpp
@@ -3,418 +3,285 @@
 #include "Replay.h"
 
 #include <AP_HAL_Linux/Scheduler.h>
+#include <AP_DAL/AP_DAL.h>
 
 #include <cinttypes>
 
 extern const AP_HAL::HAL& hal;
 
-LR_MsgHandler::LR_MsgHandler(struct log_Format &_f,
-                             AP_Logger &_logger,
-                             uint64_t &_last_timestamp_usec) :
-    logger(_logger), last_timestamp_usec(_last_timestamp_usec),
+#define MSG_CAST(sname,msg) *((log_ ##sname *)(msg+3))
+
+LR_MsgHandler::LR_MsgHandler(struct log_Format &_f) :
     MsgHandler(_f) {
 }
 
-void LR_MsgHandler::wait_timestamp_usec(uint64_t timestamp)
+void LR_MsgHandler_RFRH::process_message(uint8_t *msg)
 {
-    last_timestamp_usec = timestamp;
-    hal.scheduler->stop_clock(timestamp);
+    AP::dal().handle_message(MSG_CAST(RFRH,msg));
 }
 
-void LR_MsgHandler::wait_timestamp(uint32_t timestamp)
+void LR_MsgHandler_RFRF::process_message(uint8_t *msg)
 {
-    uint64_t usecs = timestamp*1000UL;
-    wait_timestamp_usec(usecs);
-}
-
-void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
-{
-    uint64_t time_us;
-    uint32_t time_ms;
-
-    if (field_value(msg, "TimeUS", time_us)) {
-        // 64-bit timestamp present - great!
-        wait_timestamp_usec(time_us);
-    } else if (field_value(msg, "TimeMS", time_ms)) {
-        // there is special rounding code that needs to be crossed in
-        // wait_timestamp:
-        wait_timestamp(time_ms);
-    } else {
-        ::printf("No timestamp on message");
+    const log_RFRF &RFRF = MSG_CAST(RFRF,msg);
+    uint8_t frame_types = RFRF.frame_types;
+    if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF2)) {
+        ekf2_init_done = ekf2.InitialiseFilter();
     }
-}
-
-
-
-/*
- * subclasses to handle specific messages below here
-*/
-
-void LR_MsgHandler_AHR2::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    attitude_from_msg(msg, ahr2_attitude, "Roll", "Pitch", "Yaw");
-}
-
-
-void LR_MsgHandler_ARM::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    uint8_t ArmState = require_field_uint8_t(msg, "ArmState");
-    hal.util->set_soft_armed(ArmState);
-    printf("Armed state: %u at %lu\n", 
-           (unsigned)ArmState,
-           (unsigned long)AP_HAL::millis());
-}
-
-
-void LR_MsgHandler_ARSP::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-
-    airspeed.setHIL(require_field_float(msg, "Airspeed"),
-		    require_field_float(msg, "DiffPress"),
-		    require_field_float(msg, "Temp"));
-}
-
-void LR_MsgHandler_NKF1::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-}
-
-void LR_MsgHandler_NKF1::process_message(uint8_t *msg, uint8_t &core)
-{
-    wait_timestamp_from_msg(msg);
-    if (!field_value(msg, "C", core)) {
-        // 255 here is a special marker for "no core present in log".
-        // This may give us a hope of backwards-compatability.
-        core = 255;
-    }
-}
-
-void LR_MsgHandler_XKF1::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-}
-
-void LR_MsgHandler_XKF1::process_message(uint8_t *msg, uint8_t &core)
-{
-    wait_timestamp_from_msg(msg);
-    if (!field_value(msg, "C", core)) {
-        // 255 here is a special marker for "no core present in log".
-        // This may give us a hope of backwards-compatability.
-        core = 255;
-    }
-}
-
-
-void LR_MsgHandler_ATT::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw");
-}
-
-void LR_MsgHandler_CHEK::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    check_state.time_us = AP_HAL::micros64();
-    attitude_from_msg(msg, check_state.euler, "Roll", "Pitch", "Yaw");
-    check_state.euler *= radians(1);
-    location_from_msg(msg, check_state.pos, "Lat", "Lng", "Alt");
-    require_field(msg, "VN", check_state.velocity.x);
-    require_field(msg, "VE", check_state.velocity.y);
-    require_field(msg, "VD", check_state.velocity.z);
-}
-
-
-void LR_MsgHandler_BARO::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    uint32_t last_update_ms;
-    if (!field_value(msg, "SMS", last_update_ms)) {
-        last_update_ms = 0;
-    }
-    AP::baro().setHIL(0,
-		require_field_float(msg, "Press"),
-		require_field_int16_t(msg, "Temp") * 0.01f,
-		require_field_float(msg, "Alt"),
-		require_field_float(msg, "CRt"),
-                last_update_ms);
-}
-
-
-void LR_MsgHandler_Event::process_message(uint8_t *msg)
-{
-    uint8_t id = require_field_uint8_t(msg, "Id");
-    if ((LogEvent)id == LogEvent::ARMED) {
-        hal.util->set_soft_armed(true);
-        printf("Armed at %lu\n", 
-               (unsigned long)AP_HAL::millis());
-    } else if ((LogEvent)id == LogEvent::DISARMED) {
-        hal.util->set_soft_armed(false);
-        printf("Disarmed at %lu\n", 
-               (unsigned long)AP_HAL::millis());
-    }
-}
-
-
-void LR_MsgHandler_GPS2::process_message(uint8_t *msg)
-{
-    update_from_msg_gps(1, msg);
-}
-
-void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg)
-{
-    uint64_t time_us;
-    if (! field_value(msg, "TimeUS", time_us)) {
-        uint32_t timestamp;
-        require_field(msg, "T", timestamp);
-        time_us = timestamp * 1000;
-    }
-    wait_timestamp_usec(time_us);
-
-    Location loc;
-    location_from_msg(msg, loc, "Lat", "Lng", "Alt");
-    Vector3f vel;
-    ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");
-
-    uint8_t status = require_field_uint8_t(msg, "Status");
-    uint8_t hdop = 0;
-    if (! field_value(msg, "HDop", hdop) &&
-        ! field_value(msg, "HDp", hdop)) {
-        hdop = 20;
-    }
-    uint8_t nsats = 0;
-    if (! field_value(msg, "NSats", nsats) &&
-        ! field_value(msg, "numSV", nsats)) {
-        field_not_found(msg, "NSats");
-    }
-    uint16_t GWk;
-    uint32_t GMS;
-    if (! field_value(msg, "GWk", GWk)) {
-        field_not_found(msg, "GWk");
-    }
-    if (! field_value(msg, "GMS", GMS)) {
-        field_not_found(msg, "GMS");
-    }
-    gps.setHIL(gps_offset,
-               (AP_GPS::GPS_Status)status,
-               AP_GPS::time_epoch_convert(GWk, GMS),
-               loc,
-               vel,
-               nsats,
-               hdop);
-    if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
-        ground_alt_cm = require_field_int32_t(msg, "Alt");
-    }
-
-    // we don't call GPS update_instance which would ordinarily write
-    // these...
-    AP::logger().Write_GPS(gps_offset);
-}
-
-
-
-void LR_MsgHandler_GPS::process_message(uint8_t *msg)
-{
-    update_from_msg_gps(0, msg);
-}
-
-
-void LR_MsgHandler_GPA_Base::update_from_msg_gpa(uint8_t gps_offset, uint8_t *msg)
-{
-    uint64_t time_us;
-    require_field(msg, "TimeUS", time_us);
-    wait_timestamp_usec(time_us);
-
-    uint16_t vdop, hacc, vacc, sacc;
-    require_field(msg, "VDop", vdop);
-    require_field(msg, "HAcc", hacc);
-    require_field(msg, "VAcc", vacc);
-    require_field(msg, "SAcc", sacc);
-    uint8_t have_vertical_velocity;
-    if (! field_value(msg, "VV", have_vertical_velocity)) {
-        have_vertical_velocity = !is_zero(gps.velocity(gps_offset).z);
-    }
-    uint32_t sample_ms;
-    if (! field_value(msg, "SMS", sample_ms)) {
-        sample_ms = 0;
-    }
-
-    gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f, have_vertical_velocity, sample_ms);
-}
-
-void LR_MsgHandler_GPA::process_message(uint8_t *msg)
-{
-    update_from_msg_gpa(0, msg);
-}
-
-
-void LR_MsgHandler_GPA2::process_message(uint8_t *msg)
-{
-    update_from_msg_gpa(1, msg);
-}
-
-
-
-void LR_MsgHandler_IMU2::process_message(uint8_t *msg)
-{
-  update_from_msg_imu(1, msg);
-}
-
-
-void LR_MsgHandler_IMU3::process_message(uint8_t *msg)
-{
-  update_from_msg_imu(2, msg);
-}
-
-
-void LR_MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-
-    uint8_t this_imu_mask = 1 << imu_offset;
-
-    if (gyro_mask & this_imu_mask) {
-        Vector3f gyro;
-        require_field(msg, "Gyr", gyro);
-        ins.set_gyro(imu_offset, gyro);
-    }
-    if (accel_mask & this_imu_mask) {
-        Vector3f accel2;
-        require_field(msg, "Acc", accel2);
-        ins.set_accel(imu_offset, accel2);
-    }
-}
-
-
-void LR_MsgHandler_IMU::process_message(uint8_t *msg)
-{
-    update_from_msg_imu(0, msg);
-}
-
-void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-
-    if (!use_imt) {
-        return;
-    }
-
-    uint8_t this_imu_mask = 1 << imu_offset;
-
-    float delta_time = 0;
-    require_field(msg, "DelT", delta_time);
-    ins.set_delta_time(delta_time);
-
-    if (gyro_mask & this_imu_mask) {
-        Vector3f d_angle;
-        require_field(msg, "DelA", d_angle);
-        float d_angle_dt;
-        if (!field_value(msg, "DelaT", d_angle_dt)) {
-            d_angle_dt = 0;
+    if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF2)) {
+        if (!ekf2_init_done) {
+            ekf2_init_done = ekf2.InitialiseFilter();
+        }
+        if (ekf2_init_done) {
+            ekf2.UpdateFilter();
         }
-        ins.set_delta_angle(imu_offset, d_angle, d_angle_dt);
     }
-    if (accel_mask & this_imu_mask) {
-        float dvt = 0;
-        require_field(msg, "DelvT", dvt);
-        Vector3f d_velocity;
-        require_field(msg, "DelV", d_velocity);
-        ins.set_delta_velocity(imu_offset, dvt, d_velocity);
+    if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF3)) {
+        ekf3_init_done = ekf3.InitialiseFilter();
+    }
+    if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF3)) {
+        if (!ekf3_init_done) {
+            ekf3_init_done = ekf3.InitialiseFilter();
+        }
+        if (ekf3_init_done) {
+            ekf3.UpdateFilter();
+        }
+    }
+    if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF2)) {
+        ekf2.Log_Write();
+    }
+    if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF3)) {
+        ekf3.Log_Write();
     }
 }
 
-void LR_MsgHandler_IMT::process_message(uint8_t *msg)
+void LR_MsgHandler_RFRN::process_message(uint8_t *msg)
 {
-  update_from_msg_imt(0, msg);
+    AP::dal().handle_message(MSG_CAST(RFRN,msg));
 }
 
-void LR_MsgHandler_IMT2::process_message(uint8_t *msg)
+void LR_MsgHandler_REV2::process_message(uint8_t *msg)
 {
-  update_from_msg_imt(1, msg);
-}
+    const log_REV2 &rev2 = MSG_CAST(REV2,msg);
 
-void LR_MsgHandler_IMT3::process_message(uint8_t *msg)
-{
-  update_from_msg_imt(2, msg);
-}
+    switch ((AP_DAL::Event2)rev2.event) {
 
-void LR_MsgHandler_MAG2::process_message(uint8_t *msg)
-{
-    update_from_msg_compass(1, msg);
-}
-
-
-void LR_MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-
-    Vector3f mag;
-    require_field(msg, "Mag", mag);
-    Vector3f mag_offset;
-    require_field(msg, "Ofs", mag_offset);
-    uint32_t last_update_usec;
-    if (!field_value(msg, "S", last_update_usec)) {
-        last_update_usec = AP_HAL::micros();
+    case AP_DAL::Event2::resetGyroBias:
+        ekf2.resetGyroBias();
+        break;
+    case AP_DAL::Event2::resetHeightDatum:
+        ekf2.resetHeightDatum();
+        break;
+    case AP_DAL::Event2::setInhibitGPS:
+        ekf2.setInhibitGPS();
+        break;
+    case AP_DAL::Event2::setTakeoffExpected:
+        ekf2.setTakeoffExpected(true);
+        break;
+    case AP_DAL::Event2::unsetTakeoffExpected:
+        ekf2.setTakeoffExpected(false);
+        break;
+    case AP_DAL::Event2::setTouchdownExpected:
+        ekf2.setTouchdownExpected(true);
+        break;
+    case AP_DAL::Event2::unsetTouchdownExpected:
+        ekf2.setTouchdownExpected(false);
+        break;
+    case AP_DAL::Event2::setInhibitGpsVertVelUse:
+        ekf2.setInhibitGpsVertVelUse(true);
+        break;
+    case AP_DAL::Event2::unsetInhibitGpsVertVelUse:
+        ekf2.setInhibitGpsVertVelUse(false);
+        break;
+    case AP_DAL::Event2::setTerrainHgtStable:
+        ekf2.setTerrainHgtStable(true);
+        break;
+    case AP_DAL::Event2::unsetTerrainHgtStable:
+        ekf2.setTerrainHgtStable(false);
+        break;
+    case AP_DAL::Event2::requestYawReset:
+        ekf2.requestYawReset();
+        break;
+    case AP_DAL::Event2::checkLaneSwitch:
+        ekf2.checkLaneSwitch();
+        break;
     }
+}
 
-    compass.setHIL(compass_offset, mag - mag_offset, last_update_usec);
-    // compass_offset is which compass we are setting info for;
-    // mag_offset is a vector indicating the compass' calibration...
-    compass.set_offsets(compass_offset, mag_offset);
+void LR_MsgHandler_RSO2::process_message(uint8_t *msg)
+{
+    const log_RSO2 &rso2 = MSG_CAST(RSO2,msg);
+    Location loc;
+    loc.lat = rso2.lat;
+    loc.lng = rso2.lng;
+    loc.alt = rso2.alt;
+    ekf2.setOriginLLH(loc);
+}
+
+void LR_MsgHandler_RWA2::process_message(uint8_t *msg)
+{
+    const log_RWA2 &rwa2 = MSG_CAST(RWA2,msg);
+    ekf2.writeDefaultAirSpeed(rwa2.airspeed);
 }
 
 
-
-void LR_MsgHandler_MAG::process_message(uint8_t *msg)
+void LR_MsgHandler_REV3::process_message(uint8_t *msg)
 {
-    update_from_msg_compass(0, msg);
+    const log_REV3 &rev3 = MSG_CAST(REV3,msg);
+
+    switch ((AP_DAL::Event3)rev3.event) {
+
+    case AP_DAL::Event3::resetGyroBias:
+        ekf3.resetGyroBias();
+        break;
+    case AP_DAL::Event3::resetHeightDatum:
+        ekf3.resetHeightDatum();
+        break;
+    case AP_DAL::Event3::setInhibitGPS:
+        ekf3.setInhibitGPS();
+        break;
+    case AP_DAL::Event3::setTakeoffExpected:
+        ekf3.setTakeoffExpected(true);
+        break;
+    case AP_DAL::Event3::unsetTakeoffExpected:
+        ekf3.setTakeoffExpected(false);
+        break;
+    case AP_DAL::Event3::setTouchdownExpected:
+        ekf3.setTouchdownExpected(true);
+        break;
+    case AP_DAL::Event3::unsetTouchdownExpected:
+        ekf3.setTouchdownExpected(false);
+        break;
+    case AP_DAL::Event3::setInhibitGpsVertVelUse:
+        ekf3.setInhibitGpsVertVelUse(true);
+        break;
+    case AP_DAL::Event3::unsetInhibitGpsVertVelUse:
+        ekf3.setInhibitGpsVertVelUse(false);
+        break;
+    case AP_DAL::Event3::setTerrainHgtStable:
+        ekf3.setTerrainHgtStable(true);
+        break;
+    case AP_DAL::Event3::unsetTerrainHgtStable:
+        ekf3.setTerrainHgtStable(false);
+        break;
+    case AP_DAL::Event3::requestYawReset:
+        ekf3.requestYawReset();
+        break;
+    case AP_DAL::Event3::checkLaneSwitch:
+        ekf3.checkLaneSwitch();
+        break;
+    }
+}
+
+void LR_MsgHandler_RSO3::process_message(uint8_t *msg)
+{
+    const log_RSO3 &rso3 = MSG_CAST(RSO3,msg);
+    Location loc;
+    loc.lat = rso3.lat;
+    loc.lng = rso3.lng;
+    loc.alt = rso3.alt;
+    ekf3.setOriginLLH(loc);
+}
+
+void LR_MsgHandler_RWA3::process_message(uint8_t *msg)
+{
+    const log_RWA3 &rwa3 = MSG_CAST(RWA3,msg);
+    ekf3.writeDefaultAirSpeed(rwa3.airspeed);
+}
+
+void LR_MsgHandler_REY3::process_message(uint8_t *msg)
+{
+    const log_RWA3 &rwa3 = MSG_CAST(RWA3,msg);
+    ekf3.writeDefaultAirSpeed(rwa3.airspeed);
+}
+
+void LR_MsgHandler_RISH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RISH,msg));
+}
+void LR_MsgHandler_RISI::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RISI,msg));
+}
+
+void LR_MsgHandler_RASH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RASH,msg));
+}
+void LR_MsgHandler_RASI::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RASI,msg));
+}
+
+void LR_MsgHandler_RBRH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RBRH,msg));
+}
+void LR_MsgHandler_RBRI::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RBRI,msg));
+}
+
+void LR_MsgHandler_RRNH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RRNH,msg));
+}
+void LR_MsgHandler_RRNI::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RRNI,msg));
+}
+
+void LR_MsgHandler_RGPH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RGPH,msg));
+}
+void LR_MsgHandler_RGPI::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RGPI,msg));
+}
+void LR_MsgHandler_RGPJ::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RGPJ,msg));
+}
+
+void LR_MsgHandler_RMGH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RMGH,msg));
+}
+void LR_MsgHandler_RMGI::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RMGI,msg));
+}
+
+void LR_MsgHandler_RBCH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RBCH,msg));
+}
+void LR_MsgHandler_RBCI::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RBCI,msg));
+}
+
+void LR_MsgHandler_RVOH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(RVOH,msg));
 }
 
 #include <AP_AHRS/AP_AHRS.h>
 #include "VehicleType.h"
 
-void LR_MsgHandler_MSG::process_message(uint8_t *msg)
-{
-    const uint8_t msg_text_len = 64;
-    char msg_text[msg_text_len];
-    require_field(msg, "Message", msg_text, msg_text_len);
-
-    if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
-	vehicle = VehicleType::VEHICLE_PLANE;
-	::printf("Detected Plane\n");
-	ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
-	ahrs.set_fly_forward(true);
-    } else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 ||
-	       strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) {
-	vehicle = VehicleType::VEHICLE_COPTER;
-	::printf("Detected Copter\n");
-	ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
-	ahrs.set_fly_forward(false);
-    } else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) {
-	vehicle = VehicleType::VEHICLE_ROVER;
-	::printf("Detected Rover\n");
-	ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
-	ahrs.set_fly_forward(true);
-    }
-}
-
-
-void LR_MsgHandler_NTUN_Copter::process_message(uint8_t *msg)
-{
-    inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f,
-		       require_field_float(msg, "PosY") * 0.01f,
-		       0);
-}
-
-
 bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
 {
-    const char *ignore_parms[] = { "GPS_TYPE", "AHRS_EKF_TYPE", "EK2_ENABLE", "EK3_ENABLE"
-                                   "COMPASS_ORIENT", "COMPASS_ORIENT2",
-                                   "COMPASS_ORIENT3", "LOG_FILE_BUFSIZE",
-                                   "LOG_DISARMED"};
+    const char *ignore_parms[] = {
+        // "GPS_TYPE",
+        // "AHRS_EKF_TYPE",
+        // "EK2_ENABLE",
+        // "EK3_ENABLE",
+        // "COMPASS_ORIENT",
+        // "COMPASS_ORIENT2",
+        // "COMPASS_ORIENT3",
+        "LOG_FILE_BUFSIZE",
+        "LOG_DISARMED"
+    };
     for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) {
         if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
             ::printf("Ignoring set of %s to %f\n", name, value);
@@ -432,7 +299,6 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
     uint64_t time_us;
 
     if (field_value(msg, "TimeUS", time_us)) {
-        wait_timestamp_usec(time_us);
     } else {
         // older logs can have a lot of FMT and PARM messages up the
         // front which don't have timestamps.  Since in Replay we run
@@ -445,24 +311,9 @@ void LR_MsgHandler_PARM::process_message(uint8_t *msg)
     require_field(msg, "Name", parameter_name, parameter_name_len);
 
     float value = require_field_float(msg, "Value");
-    if (globals.no_params || replay.check_user_param(parameter_name)) {
-        printf("Not changing %s to %f\n", parameter_name, value);
-    } else {
-        set_parameter(parameter_name, value);
-    }
-}
-
-void LR_MsgHandler_PM::process_message(uint8_t *msg)
-{
-    uint32_t new_logdrop;
-    if (field_value(msg, "LogDrop", new_logdrop) &&
-        new_logdrop != 0) {
-        printf("PM.LogDrop: %u dropped at timestamp %" PRIu64 "\n", new_logdrop, last_timestamp_usec);
-    }
-}
-
-void LR_MsgHandler_SIM::process_message(uint8_t *msg)
-{
-    wait_timestamp_from_msg(msg);
-    attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw");
+    // if (globals.no_params || replay.check_user_param(parameter_name)) {
+    //     printf("Not changing %s to %f\n", parameter_name, value);
+    // } else {
+    set_parameter(parameter_name, value);
+    // }
 }
diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h
index 871ace5b00..8d8f605135 100644
--- a/Tools/Replay/LR_MsgHandler.h
+++ b/Tools/Replay/LR_MsgHandler.h
@@ -3,14 +3,14 @@
 #include "MsgHandler.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_GPS/AP_GPS.h>
+#include <AP_NavEKF2/AP_NavEKF2.h>
+#include <AP_NavEKF3/AP_NavEKF3.h>
 
 #include <functional>
 
 class LR_MsgHandler : public MsgHandler {
 public:
-    LR_MsgHandler(struct log_Format &f,
-                  AP_Logger &_logger,
-                  uint64_t &last_timestamp_usec);
+    LR_MsgHandler(struct log_Format &f);
     virtual void process_message(uint8_t *msg) = 0;
     virtual void process_message(uint8_t *msg, uint8_t &core) {
         // base implementation just ignores the core parameter;
@@ -28,431 +28,233 @@ public:
     };
 
 protected:
-    AP_Logger &logger;
-    void wait_timestamp(uint32_t timestamp);
-    void wait_timestamp_usec(uint64_t timestamp);
-    void wait_timestamp_from_msg(uint8_t *msg);
-
-    uint64_t &last_timestamp_usec;
 
 };
 
-/* subclasses below this point */
-
-class LR_MsgHandler_AHR2 : public LR_MsgHandler
+class LR_MsgHandler_RFRH : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_AHR2(log_Format &_f, AP_Logger &_logger,
-                    uint64_t &_last_timestamp_usec, Vector3f &_ahr2_attitude)
-        : LR_MsgHandler(_f, _logger,_last_timestamp_usec),
-          ahr2_attitude(_ahr2_attitude) { };
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
 
+class LR_MsgHandler_RFRF : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_RFRF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
+        LR_MsgHandler(_f),
+        ekf2(_ekf2),
+        ekf3(_ekf3) {}
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+private:
+    NavEKF2 &ekf2;
+    NavEKF3 &ekf3;
+    bool ekf2_init_done;
+    bool ekf3_init_done;
+};
+
+class LR_MsgHandler_RFRN : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_REV2 : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_REV2(struct log_Format &_f, NavEKF2 &_ekf2) :
+        LR_MsgHandler(_f),
+        ekf2(_ekf2) {}
     void process_message(uint8_t *msg) override;
 
 private:
-    Vector3f &ahr2_attitude;
+    NavEKF2 &ekf2;
 };
 
-
-class LR_MsgHandler_ARM : public LR_MsgHandler
+class LR_MsgHandler_RSO2 : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_ARM(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
-
-    void process_message(uint8_t *msg) override;
-};
-
-
-class LR_MsgHandler_ARSP : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_ARSP(log_Format &_f, AP_Logger &_logger,
-		    uint64_t &_last_timestamp_usec, AP_Airspeed &_airspeed) :
-	LR_MsgHandler(_f, _logger, _last_timestamp_usec), airspeed(_airspeed) { };
-
+    LR_MsgHandler_RSO2(struct log_Format &_f, NavEKF2 &_ekf2) :
+        LR_MsgHandler(_f),
+        ekf2(_ekf2) {}
     void process_message(uint8_t *msg) override;
 
 private:
-    AP_Airspeed &airspeed;
+    NavEKF2 &ekf2;
 };
 
-class LR_MsgHandler_NKF1 : public LR_MsgHandler
+class LR_MsgHandler_RWA2 : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_NKF1(log_Format &_f, AP_Logger &_logger,
-		    uint64_t &_last_timestamp_usec) :
-	LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
-
-    void process_message(uint8_t *msg) override;
-    void process_message(uint8_t *msg, uint8_t &core) override;
-};
-
-class LR_MsgHandler_XKF1 : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_XKF1(log_Format &_f, AP_Logger &_logger,
-		    uint64_t &_last_timestamp_usec) :
-	LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
-
-    void process_message(uint8_t *msg) override;
-    void process_message(uint8_t *msg, uint8_t &core) override;
-};
-
-class LR_MsgHandler_ATT : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_ATT(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec, Vector3f &_attitude)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec), attitude(_attitude)
-        { };
+    LR_MsgHandler_RWA2(struct log_Format &_f, NavEKF2 &_ekf2) :
+        LR_MsgHandler(_f),
+        ekf2(_ekf2) {}
     void process_message(uint8_t *msg) override;
 
 private:
-    Vector3f &attitude;
+    NavEKF2 &ekf2;
 };
 
 
-class LR_MsgHandler_CHEK : public LR_MsgHandler
+class LR_MsgHandler_REV3 : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_CHEK(log_Format &_f, AP_Logger &_logger,
-                       uint64_t &_last_timestamp_usec, CheckState &_check_state)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec), 
-          check_state(_check_state)
-        { };
+    LR_MsgHandler_REV3(struct log_Format &_f, NavEKF3 &_ekf3) :
+        LR_MsgHandler(_f),
+        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
 
 private:
-    CheckState &check_state;
+    NavEKF3 &ekf3;
 };
 
-class LR_MsgHandler_BARO : public LR_MsgHandler
+class LR_MsgHandler_RSO3 : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_BARO(log_Format &_f, AP_Logger &_logger,
-                    uint64_t &_last_timestamp_usec)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec)
-        { };
-
-    void process_message(uint8_t *msg) override;
-
-};
-
-
-class LR_MsgHandler_Event : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_Event(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
-
-    void process_message(uint8_t *msg) override;
-};
-
-
-
-
-class LR_MsgHandler_GPS_Base : public LR_MsgHandler
-{
-
-public:
-    LR_MsgHandler_GPS_Base(log_Format &_f, AP_Logger &_logger,
-                           uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                           uint32_t &_ground_alt_cm)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec),
-          gps(_gps), ground_alt_cm(_ground_alt_cm) { };
-
-protected:
-    void update_from_msg_gps(uint8_t imu_offset, uint8_t *data);
-
-private:
-    AP_GPS &gps;
-    uint32_t &ground_alt_cm;
-};
-
-class LR_MsgHandler_GPS : public LR_MsgHandler_GPS_Base
-{
-public:
-    LR_MsgHandler_GPS(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                   uint32_t &_ground_alt_cm)
-        : LR_MsgHandler_GPS_Base(_f, _logger,_last_timestamp_usec,
-                              _gps, _ground_alt_cm),
-        gps(_gps), ground_alt_cm(_ground_alt_cm) { };
-
+    LR_MsgHandler_RSO3(struct log_Format &_f, NavEKF3 &_ekf3) :
+        LR_MsgHandler(_f),
+        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
 
 private:
-    AP_GPS &gps;
-    uint32_t &ground_alt_cm;
+    NavEKF3 &ekf3;
 };
 
-// it would be nice to use the same parser for both GPS message types
-// (and other packets, too...).  I*think* the contructor can simply
-// take e.g. &gps[1]... problems are going to arise if we don't
-// actually have that many gps' compiled in!
-class LR_MsgHandler_GPS2 : public LR_MsgHandler_GPS_Base
+class LR_MsgHandler_RWA3 : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_GPS2(log_Format &_f, AP_Logger &_logger,
-                    uint64_t &_last_timestamp_usec, AP_GPS &_gps,
-                    uint32_t &_ground_alt_cm)
-        : LR_MsgHandler_GPS_Base(_f, _logger, _last_timestamp_usec,
-                                 _gps, _ground_alt_cm), gps(_gps),
-        ground_alt_cm(_ground_alt_cm) { };
-    void process_message(uint8_t *msg) override;
-private:
-    AP_GPS &gps;
-    uint32_t &ground_alt_cm;
-};
-
-class LR_MsgHandler_GPA_Base : public LR_MsgHandler
-{
-
-public:
-    LR_MsgHandler_GPA_Base(log_Format &_f, AP_Logger &_logger,
-                           uint64_t &_last_timestamp_usec, AP_GPS &_gps)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec), gps(_gps) { };
-
-protected:
-    void update_from_msg_gpa(uint8_t imu_offset, uint8_t *data);
-
-private:
-    AP_GPS &gps;
-};
-
-
-class LR_MsgHandler_GPA : public LR_MsgHandler_GPA_Base
-{
-public:
-    LR_MsgHandler_GPA(log_Format &_f, AP_Logger &_logger,
-                      uint64_t &_last_timestamp_usec, AP_GPS &_gps)
-        : LR_MsgHandler_GPA_Base(_f, _logger,_last_timestamp_usec,
-                              _gps), gps(_gps) { };
-
+    LR_MsgHandler_RWA3(struct log_Format &_f, NavEKF3 &_ekf3) :
+        LR_MsgHandler(_f),
+        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
 
 private:
-    AP_GPS &gps;
+    NavEKF3 &ekf3;
 };
 
-class LR_MsgHandler_GPA2 : public LR_MsgHandler_GPA_Base
+class LR_MsgHandler_REY3 : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_GPA2(log_Format &_f, AP_Logger &_logger,
-                       uint64_t &_last_timestamp_usec, AP_GPS &_gps)
-        : LR_MsgHandler_GPA_Base(_f, _logger, _last_timestamp_usec,
-                                 _gps), gps(_gps) { };
-    void process_message(uint8_t *msg) override;
-private:
-    AP_GPS &gps;
-};
-
-
-
-
-
-class LR_MsgHandler_IMU_Base : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_IMU_Base(log_Format &_f, AP_Logger &_logger,
-                        uint64_t &_last_timestamp_usec,
-                        uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                        AP_InertialSensor &_ins) :
-        LR_MsgHandler(_f, _logger, _last_timestamp_usec),
-        accel_mask(_accel_mask),
-        gyro_mask(_gyro_mask),
-        ins(_ins) { };
-    void update_from_msg_imu(uint8_t imu_offset, uint8_t *msg);
-
-private:
-    uint8_t &accel_mask;
-    uint8_t &gyro_mask;
-    AP_InertialSensor &ins;
-};
-
-class LR_MsgHandler_IMU : public LR_MsgHandler_IMU_Base
-{
-public:
-    LR_MsgHandler_IMU(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec,
-                   uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                   AP_InertialSensor &_ins)
-        : LR_MsgHandler_IMU_Base(_f, _logger, _last_timestamp_usec,
-                              _accel_mask, _gyro_mask, _ins) { };
-
-    void process_message(uint8_t *msg) override;
-};
-
-class LR_MsgHandler_IMU2 : public LR_MsgHandler_IMU_Base
-{
-public:
-    LR_MsgHandler_IMU2(log_Format &_f, AP_Logger &_logger,
-                    uint64_t &_last_timestamp_usec,
-                    uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                    AP_InertialSensor &_ins)
-        : LR_MsgHandler_IMU_Base(_f, _logger, _last_timestamp_usec,
-                              _accel_mask, _gyro_mask, _ins) {};
-
-    void process_message(uint8_t *msg) override;
-};
-
-class LR_MsgHandler_IMU3 : public LR_MsgHandler_IMU_Base
-{
-public:
-    LR_MsgHandler_IMU3(log_Format &_f, AP_Logger &_logger,
-                    uint64_t &_last_timestamp_usec,
-                    uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                    AP_InertialSensor &_ins)
-        : LR_MsgHandler_IMU_Base(_f, _logger, _last_timestamp_usec,
-                              _accel_mask, _gyro_mask, _ins) {};
-
-    void process_message(uint8_t *msg) override;
-};
-
-
-class LR_MsgHandler_IMT_Base : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_IMT_Base(log_Format &_f, AP_Logger &_logger,
-                           uint64_t &_last_timestamp_usec,
-                           uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                           bool &_use_imt,
-                           AP_InertialSensor &_ins) :
-        LR_MsgHandler(_f, _logger, _last_timestamp_usec),
-        accel_mask(_accel_mask),
-        gyro_mask(_gyro_mask),
-        use_imt(_use_imt),
-        ins(_ins) { };
-    void update_from_msg_imt(uint8_t imu_offset, uint8_t *msg);
-
-private:
-    uint8_t &accel_mask;
-    uint8_t &gyro_mask;
-    bool &use_imt;
-    AP_InertialSensor &ins;
-};
-
-class LR_MsgHandler_IMT : public LR_MsgHandler_IMT_Base
-{
-public:
-    LR_MsgHandler_IMT(log_Format &_f, AP_Logger &_logger,
-                      uint64_t &_last_timestamp_usec,
-                      uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                      bool &_use_imt,
-                      AP_InertialSensor &_ins)
-        : LR_MsgHandler_IMT_Base(_f, _logger, _last_timestamp_usec,
-                                 _accel_mask, _gyro_mask, _use_imt, _ins) { };
-
-    void process_message(uint8_t *msg) override;
-};
-
-class LR_MsgHandler_IMT2 : public LR_MsgHandler_IMT_Base
-{
-public:
-    LR_MsgHandler_IMT2(log_Format &_f, AP_Logger &_logger,
-                       uint64_t &_last_timestamp_usec,
-                       uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                       bool &_use_imt,
-                       AP_InertialSensor &_ins)
-        : LR_MsgHandler_IMT_Base(_f, _logger, _last_timestamp_usec,
-                                 _accel_mask, _gyro_mask, _use_imt, _ins) { };
-
-    void process_message(uint8_t *msg) override;
-};
-
-class LR_MsgHandler_IMT3 : public LR_MsgHandler_IMT_Base
-{
-public:
-    LR_MsgHandler_IMT3(log_Format &_f, AP_Logger &_logger,
-                       uint64_t &_last_timestamp_usec,
-                       uint8_t &_accel_mask, uint8_t &_gyro_mask,
-                       bool &_use_imt,
-                       AP_InertialSensor &_ins)
-        : LR_MsgHandler_IMT_Base(_f, _logger, _last_timestamp_usec,
-                                 _accel_mask, _gyro_mask, _use_imt, _ins) { };
-
-    void process_message(uint8_t *msg) override;
-};
-
-
-class LR_MsgHandler_MAG_Base : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_MAG_Base(log_Format &_f, AP_Logger &_logger,
-                        uint64_t &_last_timestamp_usec, Compass &_compass)
-	: LR_MsgHandler(_f, _logger, _last_timestamp_usec), compass(_compass) { };
-
-protected:
-    void update_from_msg_compass(uint8_t compass_offset, uint8_t *msg);
-
-private:
-    Compass &compass;
-};
-
-class LR_MsgHandler_MAG : public LR_MsgHandler_MAG_Base
-{
-public:
-    LR_MsgHandler_MAG(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec, Compass &_compass)
-        : LR_MsgHandler_MAG_Base(_f, _logger, _last_timestamp_usec,_compass) {};
-
-    void process_message(uint8_t *msg) override;
-};
-
-class LR_MsgHandler_MAG2 : public LR_MsgHandler_MAG_Base
-{
-public:
-    LR_MsgHandler_MAG2(log_Format &_f, AP_Logger &_logger,
-                    uint64_t &_last_timestamp_usec, Compass &_compass)
-        : LR_MsgHandler_MAG_Base(_f, _logger, _last_timestamp_usec,_compass) {};
-
-    void process_message(uint8_t *msg) override;
-};
-
-
-
-class LR_MsgHandler_MSG : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_MSG(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec,
-                   VehicleType::vehicle_type &_vehicle, AP_AHRS &_ahrs) :
-        LR_MsgHandler(_f, _logger, _last_timestamp_usec),
-        vehicle(_vehicle), ahrs(_ahrs) { }
-
-
+    LR_MsgHandler_REY3(struct log_Format &_f, NavEKF3 &_ekf3) :
+        LR_MsgHandler(_f),
+        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
 
 private:
-    VehicleType::vehicle_type &vehicle;
-    AP_AHRS &ahrs;
+    NavEKF3 &ekf3;
 };
 
-
-class LR_MsgHandler_NTUN_Copter : public LR_MsgHandler
+class LR_MsgHandler_RISH : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_NTUN_Copter(log_Format &_f, AP_Logger &_logger,
-			   uint64_t &_last_timestamp_usec, Vector3f &_inavpos)
-	: LR_MsgHandler(_f, _logger, _last_timestamp_usec), inavpos(_inavpos) {};
-
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RISI : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RASH : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RASI : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
     void process_message(uint8_t *msg) override;
-
-private:
-    Vector3f &inavpos;
 };
 
+class LR_MsgHandler_RBRH : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RBRI : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_RRNH : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RRNI : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_RGPH : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RGPI : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RGPJ : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_RMGH : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RMGI : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RBCH : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+class LR_MsgHandler_RBCI : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_RVOH : public LR_MsgHandler
+{
+public:
+    using LR_MsgHandler::LR_MsgHandler;
+    void process_message(uint8_t *msg) override;
+};
 
 class LR_MsgHandler_PARM : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_PARM(log_Format &_f, AP_Logger &_logger,
+    LR_MsgHandler_PARM(log_Format &_f,
                        uint64_t &_last_timestamp_usec,
                        const std::function<bool(const char *name, const float)>&set_parameter_callback) :
-        LR_MsgHandler(_f, _logger, _last_timestamp_usec),
+        LR_MsgHandler(_f),
         _set_parameter_callback(set_parameter_callback)
         {};
 
@@ -462,32 +264,3 @@ private:
     bool set_parameter(const char *name, const float value);
     const std::function<bool(const char *name, const float)>_set_parameter_callback;
 };
-
-class LR_MsgHandler_PM : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_PM(log_Format &_f, AP_Logger &_logger,
-                     uint64_t &_last_timestamp_usec)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec) { };
-
-    void process_message(uint8_t *msg) override;
-
-private:
-
-};
-
-class LR_MsgHandler_SIM : public LR_MsgHandler
-{
-public:
-    LR_MsgHandler_SIM(log_Format &_f, AP_Logger &_logger,
-                   uint64_t &_last_timestamp_usec,
-                   Vector3f &_sim_attitude)
-        : LR_MsgHandler(_f, _logger, _last_timestamp_usec),
-          sim_attitude(_sim_attitude)
-        { };
-
-    void process_message(uint8_t *msg) override;
-
-private:
-    Vector3f &sim_attitude;
-};
diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp
index 695a8fd63e..683b73171e 100644
--- a/Tools/Replay/LogReader.cpp
+++ b/Tools/Replay/LogReader.cpp
@@ -1,25 +1,12 @@
-#include <AP_HAL/AP_HAL.h>
-#include <AP_Common/AP_Common.h>
-#include <AP_Math/AP_Math.h>
-#include <AP_Airspeed/AP_Airspeed.h>
-#include <AP_Compass/AP_Compass.h>
-#include <AP_GPS/AP_GPS.h>
-#include <AP_Compass/AP_Compass.h>
-#include <AP_Baro/AP_Baro.h>
-#include <AP_InertialSensor/AP_InertialSensor.h>
-#include <AP_Logger/AP_Logger.h>
-
-#include <AP_HAL_Linux/Scheduler.h>
-
 #include "LogReader.h"
+
+#include "MsgHandler.h"
+
 #include <stdio.h>
 #include <unistd.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 
-#include "MsgHandler.h"
-#include "Replay.h"
-
 #define DEBUG 1
 #if DEBUG
 # define debug(fmt, args...)     printf(fmt "\n", ##args)
@@ -29,109 +16,17 @@
 
 #define streq(x, y) (!strcmp(x, y))
 
-extern const AP_HAL::HAL& hal;
+// const struct LogStructure running_codes_log_structure[] = {
+//     LOG_COMMON_STRUCTURES,
+// };
 
-const struct LogStructure running_codes_log_structure[] = {
-    LOG_COMMON_STRUCTURES,
-};
-
-LogReader::LogReader(AP_AHRS &_ahrs,
-                     AP_InertialSensor &_ins,
-                     Compass &_compass,
-                     AP_GPS &_gps,
-                     AP_Airspeed &_airspeed,
-                     AP_Logger &_logger,
-                     struct LogStructure *log_structure,
-                     uint8_t log_structure_count,
-                     const char **&_nottypes):
+LogReader::LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
     AP_LoggerFileReader(),
-    vehicle(VehicleType::VEHICLE_UNKNOWN),
-    ahrs(_ahrs),
-    ins(_ins),
-    compass(_compass),
-    gps(_gps),
-    airspeed(_airspeed),
-    logger(_logger),
-    accel_mask(7),
-    gyro_mask(7),
-    last_timestamp_usec(0),
-    installed_vehicle_specific_parsers(false),
     _log_structure(log_structure),
-    nottypes(_nottypes)
+    ekf2(_ekf2),
+    ekf3(_ekf3)
 {
-    if (log_structure_count != 0) {
-        ::fprintf(stderr, "Do NOT put anything in the log_structure before passing it in here");
-        abort(); // so there.
-    }
-
-    initialise_fmt_map();
 }
-
-struct log_Format deferred_formats[LOGREADER_MAX_FORMATS];
-
-// some log entries (e.g. "NTUN") are used by the different vehicle
-// types with wildy varying payloads.  We thus can't use the same
-// parser for just any e.g. NTUN message.  We defer the registration
-// of a parser for these messages until we know what model we're
-// dealing with.
-void LogReader::maybe_install_vehicle_specific_parsers() {
-    if (! installed_vehicle_specific_parsers &&
-	vehicle != VehicleType::VEHICLE_UNKNOWN) {
-	switch(vehicle) {
-	case VehicleType::VEHICLE_COPTER:
-	    for (uint8_t i = 0; i<LOGREADER_MAX_FORMATS; i++) {
-		if (deferred_formats[i].type != 0) {
-		    msgparser[i] = new LR_MsgHandler_NTUN_Copter
-			(deferred_formats[i], logger, last_timestamp_usec,
-                         inavpos);
-		}
-	    }
-	    break;
-	case VehicleType::VEHICLE_PLANE:
-	    break;
-	case VehicleType::VEHICLE_ROVER:
-	    break;
-	case VehicleType::VEHICLE_UNKNOWN:
-	    break;
-	}
-	installed_vehicle_specific_parsers = true;
-    }
-}
-
-/*
-  messages which we will be generating, so should be discarded.
-  Additionally, FMT messages for messages NOT in this list will be
-  passed straight through to the output file, whereas FMT messages for
-  messages IN this list must come from LOG_BASE_STRUCTURES in
-  LogStructure.h
-
-  Note that there is an existing with FMTU messages, as these are
-  emitted both from the common structures at log startup and also when
-  Log_Write(...) is called for the first time for a particular format
-  name.  Since we include it in generated_names FMTU messages will not
-  be passed through from the source logs - but since the Replay code
-  does call Log_Write(...) you will end up with a small selection of
-  FMTU messages from that.
-
- */
-static const char *generated_names[] = {
-    "FMT",
-    "FMTU",
-    "NKF1", "NKF2", "NKF3", "NKF4", "NKF5", "NKF6", "NKF7", "NKF8", "NKF9", "NKF0",
-    "NKQ", "NKQ1", "NKQ2",
-    "XKF1", "XKF2", "XKF3", "XKF4", "XKF5", "XKF6", "XKF7", "XKF8", "XKF9", "XKF0",
-    "XKQ", "XKQ1", "XKQ2", "XKFD", "XKV1", "XKV2",
-    "AHR2",
-    "ORGN",
-    "POS",
-    "CHEK",
-    "IMT", "IMT2", "IMT3",
-    "MAG", "MAG2",
-    "BARO", "BAR2",
-    "GPS","GPA",
-    NULL,
-};
-
 // these names we emit from the code as normal, but are emitted using
 // Log_Write.  Thus they are not present in LOG_COMMON_STRUCTURES.  A
 // format will be written for this by the code itself the first time
@@ -139,14 +34,13 @@ static const char *generated_names[] = {
 // messages from the old log to the new log, so we need to keep a map
 // of IDs to prune out...
 static const char *log_write_names[] = {
-    "NKA",
-    "NKV",
-
-    "NKT1",
-    "NKT2",
     nullptr
 };
 
+static const char *generated_names[] = {
+    NULL
+};
+
 /*
   see if a type is in a list of types
  */
@@ -163,39 +57,6 @@ bool LogReader::in_list(const char *type, const char *list[])
     return false;
 }
 
-void LogReader::initialise_fmt_map()
-{
-    for (const char **name = generated_names;
-         *name !=nullptr;
-         name++) {
-        bool found = false;
-        for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
-            if (streq(*name, running_codes_log_structure[n].name)) {
-                const uint8_t t = running_codes_log_structure[n].msg_type;
-                mapped_msgid[t] = t;
-                found = true;
-                break;
-            }
-        }
-        if (!found) {
-            if (streq(*name, "CHEK")) {
-                // HACK: CHEK is emitted using Log_Write, so doesn't
-                // have a fixed address to pre-populate the fmt-map
-                // with....
-                continue;
-            }
-            if (strncmp(*name, "NK", 2)==0 || strncmp(*name, "XK", 2) == 0) {
-                // cope with older logs.  Really old logs only had EKF
-                // messages, newer logs have an instance number rather
-                // than having NKF1 and NKF6.
-                continue;
-            }
-            ::fprintf(stderr, "Failed to find apparently-generated-name (%s) in COMMON_LOG_STRUCTURES\n", *name);
-            abort();
-        }
-    }
-}
-
 /*
   map from an incoming format type to an outgoing format type
  */
@@ -210,7 +71,7 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
         return mapped_msgid[intype];
     }
     for (uint8_t n=next_msgid; n<255; n++) {
-        ::fprintf(stderr, "next_msgid=%u\n", n);
+        // ::fprintf(stderr, "next_msgid=%u\n", n);
         bool already_mapped = false;
         for (uint16_t i=0; i<sizeof(mapped_msgid); i++) {
             if (mapped_msgid[i] == n) {
@@ -227,6 +88,10 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
         }
         mapped_msgid[intype] = n;
         next_msgid = n+1;
+        if (next_msgid == 128) {
+            // don't.  Just... don't...
+            next_msgid += 1;
+        }
         break;
     }
     if (mapped_msgid[intype] == 0) {
@@ -237,46 +102,39 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
     return mapped_msgid[intype];    
 }
 
-bool LogReader::save_message_type(const char *name)
-{
-    bool save_message = !in_list(name, generated_names);
-    save_message = save_message && !in_list(name, log_write_names);
-    if (save_chek_messages && strcmp(name, "CHEK") == 0) {
-        save_message = true;
-    }
-    return save_message;
-}
-
 bool LogReader::handle_log_format_msg(const struct log_Format &f) 
 {
+    // emit the output as we receive it:
+    AP::logger().WriteBlock((void*)&f, sizeof(f));
+
 	char name[5];
 	memset(name, '\0', 5);
 	memcpy(name, f.name, 4);
-	debug("Defining log format for type (%d) (%s)\n", f.type, name);
+	// debug("Defining log format for type (%d) (%s)\n", f.type, name);
 
     struct LogStructure s = _log_structure[_log_structure_count++];
-    logger.set_num_types(_log_structure_count);
+    // logger.set_num_types(_log_structure_count);
 
     if (in_list(name, log_write_names)) {
-        debug("%s is a Log_Write-written message\n", name);
+        // debug("%s is a Log_Write-written message\n", name);
     } else {
         if (in_list(name, generated_names)) {
-            debug("Log format for type (%d) (%s) taken from running code\n",
-                  f.type, name);
-            bool found = false;
-            for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
-                if (streq(name, running_codes_log_structure[n].name)) {
-                    found = true;
-                    memcpy(&s, &running_codes_log_structure[n], sizeof(LogStructure));
-                    break;
-                }
-            }
-            if (!found) {
-                ::fprintf(stderr, "Expected to be able to emit an FMT for (%s), but no FMT message found in running code\n", name);
-                abort();
-            }
+            // debug("Log format for type (%d) (%s) taken from running code\n",
+            //       f.type, name);
+            // bool found = false;
+            // for (uint8_t n=0; n<ARRAY_SIZE(running_codes_log_structure); n++) {
+            //     if (streq(name, running_codes_log_structure[n].name)) {
+            //         found = true;
+            //         memcpy(&s, &running_codes_log_structure[n], sizeof(LogStructure));
+            //         break;
+            //     }
+            // }
+            // if (!found) {
+            //     ::fprintf(stderr, "Expected to be able to emit an FMT for (%s), but no FMT message found in running code\n", name);
+            //     abort();
+            // }
         } else {
-            debug("Log format for type (%d) (%s) taken from log\n", f.type, name);
+            // debug("Log format for type (%d) (%s) taken from log\n", f.type, name);
             // generate a LogStructure entry for this FMT
             s.msg_type = map_fmt_type(name, f.type);
             s.msg_len = f.length;
@@ -295,7 +153,7 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
         memcpy(pkt.name, s.name, sizeof(pkt.name));
         memcpy(pkt.format, s.format, sizeof(pkt.format));
         memcpy(pkt.labels, s.labels, sizeof(pkt.labels));
-        logger.WriteCriticalBlock(&pkt, sizeof(pkt));
+        // AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
     }
 
     if (msgparser[f.type] != NULL) {
@@ -305,154 +163,99 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
 	// map from format name to a parser subclass:
 	if (streq(name, "PARM")) {
             msgparser[f.type] = new LR_MsgHandler_PARM
-                (formats[f.type], logger,
+                (formats[f.type],
                  last_timestamp_usec,
                  [this](const char *xname, const float xvalue) {
                     return set_parameter(xname, xvalue);
                  });
-	} else if (streq(name, "GPS")) {
-	    msgparser[f.type] = new LR_MsgHandler_GPS(formats[f.type],
-                                                      logger,
-                                                      last_timestamp_usec,
-                                                      gps, ground_alt_cm);
-	} else if (streq(name, "GPS2")) {
-	    msgparser[f.type] = new LR_MsgHandler_GPS2(formats[f.type], logger,
-                                                       last_timestamp_usec,
-                                                       gps, ground_alt_cm);
-	} else if (streq(name, "GPA")) {
-	    msgparser[f.type] = new LR_MsgHandler_GPA(formats[f.type],
-                                                      logger,
-                                                      last_timestamp_usec,
-                                                      gps);
-	} else if (streq(name, "GPA2")) {
-	    msgparser[f.type] = new LR_MsgHandler_GPA2(formats[f.type], logger,
-                                                       last_timestamp_usec,
-                                                       gps);
-	} else if (streq(name, "MSG")) {
-	    msgparser[f.type] = new LR_MsgHandler_MSG(formats[f.type], logger,
-                                                   last_timestamp_usec,
-						   vehicle, ahrs);
-	} else if (streq(name, "IMU")) {
-	    msgparser[f.type] = new LR_MsgHandler_IMU(formats[f.type], logger,
-                                                   last_timestamp_usec,
-						   accel_mask, gyro_mask, ins);
-	} else if (streq(name, "IMU2")) {
-	    msgparser[f.type] = new LR_MsgHandler_IMU2(formats[f.type], logger,
-                                                    last_timestamp_usec,
-						    accel_mask, gyro_mask, ins);
-	} else if (streq(name, "IMU3")) {
-	    msgparser[f.type] = new LR_MsgHandler_IMU3(formats[f.type], logger,
-                                                    last_timestamp_usec,
-						    accel_mask, gyro_mask, ins);
-	} else if (streq(name, "IMT")) {
-	    msgparser[f.type] = new LR_MsgHandler_IMT(formats[f.type], logger,
-                                                      last_timestamp_usec,
-                                                      accel_mask, gyro_mask, use_imt, ins);
-	} else if (streq(name, "IMT2")) {
-	    msgparser[f.type] = new LR_MsgHandler_IMT2(formats[f.type], logger,
-                                                       last_timestamp_usec,
-                                                       accel_mask, gyro_mask, use_imt, ins);
-	} else if (streq(name, "IMT3")) {
-	    msgparser[f.type] = new LR_MsgHandler_IMT3(formats[f.type], logger,
-                                                       last_timestamp_usec,
-                                                       accel_mask, gyro_mask, use_imt, ins);
-	} else if (streq(name, "SIM")) {
-	  msgparser[f.type] = new LR_MsgHandler_SIM(formats[f.type], logger,
-                                                 last_timestamp_usec,
-						 sim_attitude);
-	} else if (streq(name, "BARO")) {
-	  msgparser[f.type] = new LR_MsgHandler_BARO(formats[f.type], logger,
-                                                  last_timestamp_usec);
-	} else if (streq(name, "ARM")) {
-	  msgparser[f.type] = new LR_MsgHandler_ARM(formats[f.type], logger,
-                                                  last_timestamp_usec);
-	} else if (streq(name, "EV")) {
-	  msgparser[f.type] = new LR_MsgHandler_Event(formats[f.type], logger,
-                                                  last_timestamp_usec);
-	} else if (streq(name, "AHR2")) {
-	  msgparser[f.type] = new LR_MsgHandler_AHR2(formats[f.type], logger,
-						  last_timestamp_usec,
-                                                  ahr2_attitude);
-	} else if (streq(name, "ATT")) {
-	  // this parser handles *all* attitude messages - the common one,
-	  // and also the rover/copter/plane-specific (old) messages
-	  msgparser[f.type] = new LR_MsgHandler_ATT(formats[f.type], logger,
-						 last_timestamp_usec,
-                                                 attitude);
-	} else if (streq(name, "MAG")) {
-	  msgparser[f.type] = new LR_MsgHandler_MAG(formats[f.type], logger,
-						 last_timestamp_usec, compass);
-	} else if (streq(name, "MAG2")) {
-	  msgparser[f.type] = new LR_MsgHandler_MAG2(formats[f.type], logger,
-						 last_timestamp_usec, compass);
-	} else if (streq(name, "NTUN")) {
-	    // the label "NTUN" is used by rover, copter and plane -
-	    // and they all look different!  creation of a parser is
-	    // deferred until we receive a MSG log entry telling us
-	    // which vehicle type to use.  Sucks.
-	    memcpy(&deferred_formats[f.type], &formats[f.type],
-                   sizeof(struct log_Format));
-	} else if (streq(name, "ARSP")) { // plane-specific(?!)
-	    msgparser[f.type] = new LR_MsgHandler_ARSP(formats[f.type], logger,
-                                                    last_timestamp_usec,
-                                                    airspeed);
-	} else if (streq(name, "NKF1")) {
-	    msgparser[f.type] = new LR_MsgHandler_NKF1(formats[f.type], logger,
-                                                       last_timestamp_usec);
-	} else if (streq(name, "CHEK")) {
-	  msgparser[f.type] = new LR_MsgHandler_CHEK(formats[f.type], logger,
-                                                     last_timestamp_usec,
-                                                     check_state);
-	} else if (streq(name, "PM")) {
-	  msgparser[f.type] = new LR_MsgHandler_PM(formats[f.type], logger,
-                                                   last_timestamp_usec);
+	} else if (streq(name, "RFRH")) {
+        msgparser[f.type] = new LR_MsgHandler_RFRH(formats[f.type]);
+    } else if (streq(name, "RFRF")) {
+        msgparser[f.type] = new LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3);
+    } else if (streq(name, "RFRN")) {
+        msgparser[f.type] = new LR_MsgHandler_RFRN(formats[f.type]);
+    } else if (streq(name, "REV2")) {
+	    msgparser[f.type] = new LR_MsgHandler_REV2(formats[f.type], ekf2);
+	} else if (streq(name, "RSO2")) {
+	    msgparser[f.type] = new LR_MsgHandler_RSO2(formats[f.type], ekf2);
+	} else if (streq(name, "RWA2")) {
+	    msgparser[f.type] = new LR_MsgHandler_RWA2(formats[f.type], ekf2);
+	} else if (streq(name, "REV3")) {
+	    msgparser[f.type] = new LR_MsgHandler_REV3(formats[f.type], ekf3);
+	} else if (streq(name, "RSO3")) {
+	    msgparser[f.type] = new LR_MsgHandler_RSO3(formats[f.type], ekf3);
+	} else if (streq(name, "RWA3")) {
+	    msgparser[f.type] = new LR_MsgHandler_RWA3(formats[f.type], ekf3);
+	} else if (streq(name, "REY3")) {
+	    msgparser[f.type] = new LR_MsgHandler_REY3(formats[f.type], ekf3);
+	} else if (streq(name, "RISH")) {
+	    msgparser[f.type] = new LR_MsgHandler_RISH(formats[f.type]);
+	} else if (streq(name, "RISI")) {
+	    msgparser[f.type] = new LR_MsgHandler_RISI(formats[f.type]);
+    } else if (streq(name, "RASH")) {
+	    msgparser[f.type] = new LR_MsgHandler_RASH(formats[f.type]);
+	} else if (streq(name, "RASI")) {
+	    msgparser[f.type] = new LR_MsgHandler_RASI(formats[f.type]);
+	} else if (streq(name, "RBRH")) {
+	    msgparser[f.type] = new LR_MsgHandler_RBRH(formats[f.type]);
+	} else if (streq(name, "RBRI")) {
+	    msgparser[f.type] = new LR_MsgHandler_RBRI(formats[f.type]);
+	} else if (streq(name, "RRNH")) {
+	    msgparser[f.type] = new LR_MsgHandler_RRNH(formats[f.type]);
+	} else if (streq(name, "RRNI")) {
+	    msgparser[f.type] = new LR_MsgHandler_RRNI(formats[f.type]);
+	} else if (streq(name, "RGPH")) {
+	    msgparser[f.type] = new LR_MsgHandler_RGPH(formats[f.type]);
+	} else if (streq(name, "RGPI")) {
+	    msgparser[f.type] = new LR_MsgHandler_RGPI(formats[f.type]);
+    } else if (streq(name, "RGPJ")) {
+        msgparser[f.type] = new LR_MsgHandler_RGPJ(formats[f.type]);
+	} else if (streq(name, "RMGH")) {
+	    msgparser[f.type] = new LR_MsgHandler_RMGH(formats[f.type]);
+	} else if (streq(name, "RMGI")) {
+	    msgparser[f.type] = new LR_MsgHandler_RMGI(formats[f.type]);
+	} else if (streq(name, "RBCH")) {
+	    msgparser[f.type] = new LR_MsgHandler_RBCH(formats[f.type]);
+	} else if (streq(name, "RBCI")) {
+	    msgparser[f.type] = new LR_MsgHandler_RBCI(formats[f.type]);
 	} else {
-            debug("  No parser for (%s)\n", name);
-	}
-
-        return true;
-}
-
-bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) {
-    char name[5];
-    memset(name, '\0', 5);
-    memcpy(name, f.name, 4);
-
-    if (save_message_type(name)) {
-        // write this message through to output log, changing the ID
-        // present in the input log to that used for the same message
-        // name in the output log
-        if (mapped_msgid[msg[2]] == 0) {
-            printf("Unknown msgid %u\n", (unsigned)msg[2]);
-            exit(1);
-        }
-        msg[2] = mapped_msgid[msg[2]];
-        if (!in_list(name, nottypes)) {
-            logger.WriteBlock(msg, f.length);        
-        }
-        // a MsgHandler would probably have found a timestamp and
-        // caled stop_clock.  This runs IO, clearing logger's
-        // buffer.
-        hal.scheduler->stop_clock(((Linux::Scheduler*)hal.scheduler)->stopped_clock_usec());
+        // debug("  No parser for (%s)\n", name);
     }
 
-    LR_MsgHandler *p = msgparser[f.type];
-    if (p == NULL) {
-	return true;
-    }
-
-    p->process_message(msg, core);
-
-    maybe_install_vehicle_specific_parsers();
-
     return true;
 }
 
+bool LogReader::handle_msg(const struct log_Format &f, uint8_t *msg) {
+    // emit the output as we receive it:
+    AP::logger().WriteBlock(msg, f.length);
+
+    // char name[5];
+    // memset(name, '\0', 5);
+    // memcpy(name, f.name, 4);
+
+    LR_MsgHandler *p = msgparser[f.type];
+    if (p == NULL) {
+        return true;
+    }
+
+    p->process_message(msg);
+
+    return true;
+}
+       #include <sys/types.h>
+       #include <signal.h>
+
 bool LogReader::set_parameter(const char *name, float value)
 {
+    // if (!strcmp(name, "EK3_ENABLE")) {
+    //     kill(0, SIGTRAP);
+    // }
     enum ap_var_type var_type;
     AP_Param *vp = AP_Param::find(name, &var_type);
     if (vp == NULL) {
+        // a lot of parameters will not be found - e.g. FORMAT_VERSION
+        // and all of the vehicle-specific parameters, ....
+        // ::fprintf(stderr, "Parameter (%s) not found\n", name);
         return false;
     }
     float old_value = 0;
@@ -469,11 +272,10 @@ bool LogReader::set_parameter(const char *name, float value)
         old_value = ((AP_Int8 *)vp)->cast_to_float();
         ((AP_Int8 *)vp)->set(value);
     } else {
-        // we don't support mavlink set on this parameter
-        return false;
+        AP_HAL::panic("What manner of evil is var_type=%u", var_type);
     }
     if (fabsf(old_value - value) > 1.0e-12) {
-        ::printf("Changed %s to %.8f from %.8f\n", name, value, old_value);
+        ::fprintf(stderr, "Changed %s to %.8f from %.8f\n", name, value, old_value);
     }
     return true;
 }
diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h
index 739cb3fff0..879dfe355e 100644
--- a/Tools/Replay/LogReader.h
+++ b/Tools/Replay/LogReader.h
@@ -1,3 +1,5 @@
+#pragma once
+
 #include "VehicleType.h"
 #include "DataFlashFileReader.h"
 #include "LR_MsgHandler.h"
@@ -6,15 +8,7 @@
 class LogReader : public AP_LoggerFileReader
 {
 public:
-    LogReader(AP_AHRS &_ahrs,
-              AP_InertialSensor &_ins,
-              Compass &_compass,
-              AP_GPS &_gps,
-              AP_Airspeed &_airspeed,
-              AP_Logger &_logger,
-              struct LogStructure *log_structure,
-              uint8_t log_structure_count,
-              const char **&nottypes);
+    LogReader(struct LogStructure *log_structure, NavEKF2 &_ekf, NavEKF3 &_ekf3);
 
     const Vector3f &get_attitude(void) const { return attitude; }
     const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; }
@@ -34,19 +28,17 @@ public:
 
     uint64_t last_timestamp_us(void) const { return last_timestamp_usec; }
     bool handle_log_format_msg(const struct log_Format &f) override;
-    bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override;
+    bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
 
     static bool in_list(const char *type, const char *list[]);
 
 protected:
 
 private:
-    AP_AHRS &ahrs;
-    AP_InertialSensor &ins;
-    Compass &compass;
-    AP_GPS &gps;
-    AP_Airspeed &airspeed;
-    AP_Logger &logger;
+
+    NavEKF2 &ekf2;
+    NavEKF3 &ekf3;
+
     struct LogStructure *_log_structure;
     uint8_t _log_structure_count;
 
@@ -74,7 +66,6 @@ private:
     LR_MsgHandler::CheckState check_state;
 
     bool installed_vehicle_specific_parsers;
-    const char **&nottypes;
 
     bool save_chek_messages;
 
diff --git a/Tools/Replay/Makefile b/Tools/Replay/Makefile
index 68460b8dab..255b35fcee 100644
--- a/Tools/Replay/Makefile
+++ b/Tools/Replay/Makefile
@@ -1,7 +1,7 @@
 # this is meant to make existing build instructions work with waf
 
 all:
-	@cd ../../ && modules/waf/waf-light configure --board linux --debug
+	@cd ../../ && modules/waf/waf-light configure --board linux --debug --disable-scripting
 	@cd ../../ && modules/waf/waf-light --target tools/Replay
 	@cp ../../build/linux/tools/Replay Replay.elf
 	@echo Built Replay.elf
diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp
index 476dab3f8e..6f0f484d1d 100644
--- a/Tools/Replay/MsgHandler.cpp
+++ b/Tools/Replay/MsgHandler.cpp
@@ -1,5 +1,4 @@
 #include "MsgHandler.h"
-#include <AP_AHRS/AP_AHRS.h>
 
 void fatal(const char *msg) {
     ::printf("%s",msg);
diff --git a/Tools/Replay/Parameters.h b/Tools/Replay/Parameters.h
index e5df26dc93..af17de6997 100644
--- a/Tools/Replay/Parameters.h
+++ b/Tools/Replay/Parameters.h
@@ -15,7 +15,8 @@ public:
         k_param_NavEKF2,
         k_param_compass,
         k_param_logger,
-        k_param_NavEKF3
+        k_param_NavEKF3,
+        k_param_gps,
     };
     AP_Int8 dummy;
 };
diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp
index 475483d3fc..df76380122 100644
--- a/Tools/Replay/Replay.cpp
+++ b/Tools/Replay/Replay.cpp
@@ -13,36 +13,23 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <AP_Param/AP_Param.h>
-#include <AP_HAL_Linux/Scheduler.h>
-#include <GCS_MAVLink/GCS_Dummy.h>
-#include "Parameters.h"
-#include "VehicleType.h"
-#include "MsgHandler.h"
-
-#ifndef INT16_MIN
-#define INT16_MIN -32768
-#define INT16_MAX 32767
-#endif
-
-#include "LogReader.h"
-#include "DataFlashFileReader.h"
 #include "Replay.h"
 
-#include <AP_Camera/AP_Camera.h>
+#include "LogReader.h"
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-#include <SITL/SITL.h>
-#endif
+#include <stdio.h>
+#include <AP_HAL/utility/getopt_cpp.h>
+
+#include <AP_Vehicle/AP_Vehicle.h>
+
+#include <GCS_MAVLink/GCS_Dummy.h>
+
+#include <AP_HAL_Linux/Scheduler.h>
 
 #define streq(x, y) (!strcmp(x, y))
 
-const AP_HAL::HAL& hal = AP_HAL::get_HAL();
-
 static ReplayVehicle replayvehicle;
 
-struct globals globals;
-
 #define GSCALAR(v, name, def) { replayvehicle.g.v.vtype, name, Parameters::k_param_ ## v, &replayvehicle.g.v, {def_value : def} }
 #define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &replayvehicle.v, {group_info : class::var_info} }
 #define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &replayvehicle.v, {group_info : class::var_info} }
@@ -70,7 +57,7 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
 
     // @Group: EK2_
     // @Path: ../libraries/AP_NavEKF2/AP_NavEKF2.cpp
-    GOBJECTN(ahrs.EKF2, NavEKF2, "EK2_", NavEKF2),
+    GOBJECTN(ekf2, NavEKF2, "EK2_", NavEKF2),
     
     // @Group: COMPASS_
     // @Path: ../libraries/AP_Compass/AP_Compass.cpp
@@ -82,12 +69,15 @@ const AP_Param::Info ReplayVehicle::var_info[] = {
     
     // @Group: EK3_
     // @Path: ../libraries/AP_NavEKF3/AP_NavEKF3.cpp
-    GOBJECTN(ahrs.EKF3, NavEKF3, "EK3_", NavEKF3),
+    GOBJECTN(ekf3, NavEKF3, "EK3_", NavEKF3),
 
+    // @Group: GPS_
+    // @Path: ../libraries/AP_GPS/AP_GPS.cpp
+    GOBJECT(gps, "GPS_", AP_GPS),
+    
     AP_VAREND
 };
 
-
 void ReplayVehicle::load_parameters(void)
 {
     unlink("Replay.stg");
@@ -97,36 +87,24 @@ void ReplayVehicle::load_parameters(void)
     // Load all auto-loaded EEPROM variables - also registers thread
     // which saves parameters, which Compass now does in its init() routine
     AP_Param::load_all();
-
-    // this compass has always been at war with the new compass priority code
-    const uint32_t compass_dev_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, 0, 0, AP_Compass_Backend::DevTypes::DEVTYPE_SITL);
-    // logreader.set_parameter("DEV_ID", compass_dev_id);
-    // logreader.set_parameter("PRIO1_ID", compass_dev_id);
-    AP_Param::set_default_by_name("DEV_ID", compass_dev_id);
-    AP_Param::set_default_by_name("PRIO1_ID", compass_dev_id);
-
-    const struct param_default {
-        const char *name;
-        float value;
-    } param_defaults[] {
-        { "EK2_ENABLE", 1 },
-        { "EK2_IMU_MASK", 1 },
-        { "EK3_ENABLE", 1 },
-        { "EK3_IMU_MASK", 1 },
-        { "LOG_REPLAY", 1 },
-        { "AHRS_EKF_TYPE", 2 },
-        { "LOG_FILE_BUFSIZE", 60 },
-        { "COMPASS_DEV_ID", (float)compass_dev_id },
-        { "COMPASS_PRIO1_ID", (float)compass_dev_id },
-    };
-    for (auto some_default : param_defaults) {
-        if (!AP_Param::set_default_by_name(some_default.name, some_default.value)) {
-            ::fprintf(stderr, "set default failed\n");
-            abort();
-        }
-    }
 }
 
+const struct AP_Param::GroupInfo        GCS_MAVLINK_Parameters::var_info[] = {
+    AP_GROUPEND
+};
+GCS_Dummy _gcs;
+
+AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
+bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }
+
+// dummy method to avoid linking AP_Avoidance
+// AP_Avoidance *AP::ap_avoidance() { return nullptr; }
+
+// avoid building/linking LTM:
+void AP_LTM_Telem::init() {};
+// avoid building/linking Devo:
+void AP_DEVO_Telem::init() {};
+
 void ReplayVehicle::init_ardupilot(void)
 {
     // we pass an empty log structure, filling the structure in with
@@ -135,121 +113,13 @@ void ReplayVehicle::init_ardupilot(void)
     // the current code (if we do emit the message in the normal
     // places in the EKF, for example)
     logger.Init(log_structure, 0);
-
-    ahrs.set_compass(&compass);
-    ahrs.set_fly_forward(true);
-    ahrs.set_wind_estimation(true);
-    ahrs.set_correct_centrifugal(true);
-    ahrs.set_ekf_use(true);
-
-    ahrs.EKF2.set_enable(true);
-    ahrs.EKF3.set_enable(true);
-
-    printf("Starting disarmed\n");
-    hal.util->set_soft_armed(false);
-
-    barometer.init();
-    barometer.setHIL(0);
-    barometer.update();
-    compass.init();
-    ins.set_hil_mode();
-}
-
-Replay replay(replayvehicle);
-AP_Vehicle& vehicle = replayvehicle;
-
-void Replay::usage(void)
-{
-    ::printf("Options:\n");
-    ::printf("\t--parm NAME=VALUE  set parameter NAME to VALUE\n");
-    ::printf("\t--accel-mask MASK  set accel mask (1=accel1 only, 2=accel2 only, 3=both)\n");
-    ::printf("\t--gyro-mask MASK   set gyro mask (1=gyro1 only, 2=gyro2 only, 3=both)\n");
-    ::printf("\t--arm-time time    arm at time (milliseconds)\n");
-    ::printf("\t--no-imt           don't use IMT data\n");
-    ::printf("\t--check-generate   generate CHEK messages in output\n");
-    ::printf("\t--check            check solution against CHEK messages\n");
-    ::printf("\t--tolerance-euler  tolerance for euler angles in degrees\n");
-    ::printf("\t--tolerance-pos    tolerance for position in meters\n");
-    ::printf("\t--tolerance-vel    tolerance for velocity in meters/second\n");
-    ::printf("\t--nottypes         list of msg types not to output, comma separated\n");
-    ::printf("\t--downsample       downsampling rate for output\n");
-    ::printf("\t--logmatch         match logging rate to source\n");
-    ::printf("\t--no-params        don't use parameters from the log\n");
-    ::printf("\t--no-fpe           do not generate floating point exceptions\n");
-    ::printf("\t--packet-counts    print packet counts at end of processing\n");
-}
-
-
-enum {
-    OPT_CHECK = 128,
-    OPT_CHECK_GENERATE,
-    OPT_TOLERANCE_EULER,
-    OPT_TOLERANCE_POS,
-    OPT_TOLERANCE_VEL,
-    OPT_NOTTYPES,
-    OPT_DOWNSAMPLE,
-    OPT_LOGMATCH,
-    OPT_NOPARAMS,
-    OPT_PARAM_FILE,
-    OPT_NO_FPE,
-    OPT_PACKET_COUNTS,
-};
-
-void Replay::flush_logger(void) {
-    _vehicle.logger.flush();
-}
-
-/*
-  create a list from a comma separated string
- */
-const char **Replay::parse_list_from_string(const char *str_in)
-{
-    uint16_t comma_count=0;
-    const char *p;
-    for (p=str_in; *p; p++) {
-        if (*p == ',') comma_count++;
-    }
-
-    char *str = strdup(str_in);
-    if (str == NULL) {
-        return NULL;
-    }
-    const char **ret = (const char **)calloc(comma_count+2, sizeof(char *));
-    if (ret == NULL) {
-        free(str);
-        return NULL;
-    }
-    char *saveptr = NULL;
-    uint16_t idx = 0;
-    for (p=strtok_r(str, ",", &saveptr); p; p=strtok_r(NULL, ",", &saveptr)) {
-        ret[idx++] = p;
-    }
-    return ret;
+    logger.set_force_log_disarmed(true);
 }
 
 void Replay::_parse_command_line(uint8_t argc, char * const argv[])
 {
     const struct GetOptLong::option options[] = {
         // name           has_arg flag   val
-        {"parm",            true,   0, 'p'},
-        {"param",           true,   0, 'p'},
-        {"param-file",      true,   0, OPT_PARAM_FILE},
-        {"help",            false,  0, 'h'},
-        {"accel-mask",      true,   0, 'a'},
-        {"gyro-mask",       true,   0, 'g'},
-        {"arm-time",        true,   0, 'A'},
-        {"no-imt",          false,  0, 'n'},
-        {"check-generate",  false,  0, OPT_CHECK_GENERATE},
-        {"check",           false,  0, OPT_CHECK},
-        {"tolerance-euler", true,   0, OPT_TOLERANCE_EULER},
-        {"tolerance-pos",   true,   0, OPT_TOLERANCE_POS},
-        {"tolerance-vel",   true,   0, OPT_TOLERANCE_VEL},
-        {"nottypes",        true,   0, OPT_NOTTYPES},
-        {"downsample",      true,   0, OPT_DOWNSAMPLE},
-        {"logmatch",        false,  0, OPT_LOGMATCH},
-        {"no-params",       false,  0, OPT_NOPARAMS},
-        {"no-fpe",          false,  0, OPT_NO_FPE},
-        {"packet-counts",   false,  0, OPT_PACKET_COUNTS},
         {0, false, 0, 0}
     };
 
@@ -257,272 +127,15 @@ void Replay::_parse_command_line(uint8_t argc, char * const argv[])
 
     int opt;
     while ((opt = gopt.getoption()) != -1) {
-		switch (opt) {
-        case 'g':
-            logreader.set_gyro_mask(strtol(gopt.optarg, NULL, 0));
-            break;
-
-        case 'a':
-            logreader.set_accel_mask(strtol(gopt.optarg, NULL, 0));
-            break;
-
-        case 'A':
-            arm_time_ms = strtol(gopt.optarg, NULL, 0);
-            break;
-
-        case 'n':
-            use_imt = false;
-            logreader.set_use_imt(use_imt);
-            break;
-
-        case 'p': {
-            const char *eq = strchr(gopt.optarg, '=');
-            if (eq == NULL) {
-                ::printf("Usage: -p NAME=VALUE\n");
-                exit(1);
-            }
-            struct user_parameter *u = new user_parameter;
-            strncpy(u->name, gopt.optarg, eq-gopt.optarg);
-            u->value = atof(eq+1);
-            u->next = user_parameters;
-            user_parameters = u;
-            break;
-        }
-
-        case OPT_CHECK_GENERATE:
-            check_generate = true;
-            break;
-
-        case OPT_CHECK:
-            check_solution = true;
-            break;
-
-        case OPT_TOLERANCE_EULER:
-            tolerance_euler = atof(gopt.optarg);
-            break;
-
-        case OPT_TOLERANCE_POS:
-            tolerance_pos = atof(gopt.optarg);
-            break;
-
-        case OPT_TOLERANCE_VEL:
-            tolerance_vel = atof(gopt.optarg);
-            break;
-
-        case OPT_NOTTYPES:
-            nottypes = parse_list_from_string(gopt.optarg);
-            break;
-
-        case OPT_DOWNSAMPLE:
-            downsample = atoi(gopt.optarg);
-            break;
-
-        case OPT_LOGMATCH:
-            logmatch = true;
-            break;
-
-        case OPT_NOPARAMS:
-            globals.no_params = true;
-            break;
-
-        case OPT_PARAM_FILE:
-            load_param_file(gopt.optarg);
-            break;
-            
-        case OPT_NO_FPE:
-            generate_fpe = false;
-            break;
-
-        case OPT_PACKET_COUNTS:
-            packet_counts = true;
-            break;
-
-        case 'h':
-        default:
-            usage();
-            exit(0);
-        }
     }
-
-	argv += gopt.optind;
-	argc -= gopt.optind;
+       argv += gopt.optind;
+       argc -= gopt.optind;
 
     if (argc > 0) {
         filename = argv[0];
     }
 }
 
-class IMUCounter : public AP_LoggerFileReader {
-public:
-    IMUCounter() {}
-    bool handle_log_format_msg(const struct log_Format &f) override;
-    bool handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) override;
-
-    uint64_t last_clock_timestamp = 0;
-    float last_parm_value = 0;
-    char last_parm_name[17] {};
-private:
-    MsgHandler *handler = nullptr;
-    MsgHandler *parm_handler = nullptr;
-};
-
-bool IMUCounter::handle_log_format_msg(const struct log_Format &f) {
-    if (!strncmp(f.name,"IMU",4) ||
-        !strncmp(f.name,"IMT",4)) {
-        // an IMU or IMT message message format
-        handler = new MsgHandler(f);
-    }
-    if (strncmp(f.name,"PARM",4) == 0) {
-        // PARM message message format
-        parm_handler = new MsgHandler(f);
-    }
-
-    return true;
-};
-
-bool IMUCounter::handle_msg(const struct log_Format &f, uint8_t *msg, uint8_t &core) {
-    if (strncmp(f.name,"PARM",4) == 0) {
-        // gather parameter values to check for SCHED_LOOP_RATE
-        parm_handler->field_value(msg, "Name", last_parm_name, sizeof(last_parm_name));
-        parm_handler->field_value(msg, "Value", last_parm_value);
-        return true;
-    }
-    if (strncmp(f.name,"IMU",4) &&
-        strncmp(f.name,"IMT",4)) {
-        // not an IMU message
-        return true;
-    }
-
-    if (handler->field_value(msg, "TimeUS", last_clock_timestamp)) {
-    } else if (handler->field_value(msg, "TimeMS", last_clock_timestamp)) {
-        last_clock_timestamp *= 1000;
-    } else {
-        ::printf("Unable to find timestamp in message");
-    }
-    return true;
-}
-
-/*
-  find information about the log
- */
-bool Replay::find_log_info(struct log_information &info) 
-{
-    IMUCounter reader;
-    if (!reader.open_log(filename)) {
-        perror(filename);
-        exit(1);
-    }
-    char clock_source[5] = { };
-    int samplecount = 0;
-    uint64_t prev = 0;
-    uint64_t smallest_delta = 0;
-    uint64_t total_delta = 0;
-    prev = 0;
-    const uint16_t samples_required = 1000;
-    while (samplecount < samples_required) {
-        char type[5];
-        uint8_t core; // unused
-        if (!reader.update(type, core)) {
-            break;
-        }
-
-        if (streq(type, "PARM") && streq(reader.last_parm_name, "SCHED_LOOP_RATE")) {
-            // get rate directly from parameters
-            info.update_rate = reader.last_parm_value;
-        }
-        if (strlen(clock_source) == 0) {
-            // If you want to add a clock source, also add it to
-            // handle_msg and handle_log_format_msg, above.  Note that
-            // ordering is important here.  For example, when we log
-            // IMT we may reduce the logging speed of IMU, so then
-            // using IMU as your clock source will lead to incorrect
-            // behaviour.
-            if (streq(type, "IMT")) {
-                strcpy(clock_source, "IMT");
-            } else if (streq(type, "IMU")) {
-                strcpy(clock_source, "IMU");
-            } else {
-                continue;
-            }
-            hal.console->printf("Using clock source %s\n", clock_source);
-        }
-        // IMT if available always overrides
-        if (streq(type, "IMT") && strcmp(clock_source, "IMT") != 0) {
-            strcpy(clock_source, "IMT");
-            hal.console->printf("Changing clock source to %s\n", clock_source);
-            samplecount = 0;
-            prev = 0;
-            smallest_delta = 0;
-            total_delta = 0;
-        }
-        if (streq(type, clock_source)) {
-            if (prev == 0) {
-                prev = reader.last_clock_timestamp;
-            } else {
-                uint64_t delta = reader.last_clock_timestamp - prev;
-                if (delta < 40000 && delta > 1000) {
-                    if (smallest_delta == 0 || delta < smallest_delta) {
-                        smallest_delta = delta;
-                    }
-                    samplecount++;
-                    total_delta += delta;
-                }
-            }
-            prev = reader.last_clock_timestamp;
-        }
-
-        if (streq(type, "IMU2")) {
-            info.have_imu2 = true;
-        }
-        if (streq(type, "IMT")) {
-            info.have_imt = true;
-        }
-        if (streq(type, "IMT2")) {
-            info.have_imt2 = true;
-        }
-    }
-    if (smallest_delta == 0) {
-        ::printf("Unable to determine log rate - insufficient IMU/IMT messages? (need=%d got=%d)", samples_required, samplecount);
-        return false;
-    }
-
-    float average_delta = total_delta / samplecount;
-    float rate = 1.0e6f/average_delta;
-    printf("average_delta=%.2f smallest_delta=%lu samplecount=%lu\n",
-           average_delta, (unsigned long)smallest_delta, (unsigned long)samplecount);
-    if (rate < 100) {
-        info.update_rate = 50;
-    } else {
-        info.update_rate = 400;
-    }
-    return true;
-}
-
-// catch floating point exceptions
-static void _replay_sig_fpe(int signum)
-{
-    fprintf(stderr, "ERROR: Floating point exception - flushing logger...\n");
-    replay.flush_logger();
-    fprintf(stderr, "ERROR: ... and aborting.\n");
-    if (replay.check_solution) {
-        FILE *f = fopen("replay_results.txt","a");
-        fprintf(f, "%s\tFPE\tFPE\tFPE\tFPE\tFPE\n",
-                replay.log_filename);
-        fclose(f);
-    }
-    abort();
-}
-
-FILE *Replay::xfopen(const char *f, const char *mode)
-{
-    FILE *ret = fopen(f, mode);
-    if (ret == nullptr) {
-        ::fprintf(stderr, "Failed to open (%s): %m\n", f);
-        abort();
-    }
-    return ret;
-}
-
 void Replay::setup()
 {
     ::printf("Starting\n");
@@ -534,293 +147,17 @@ void Replay::setup()
 
     _parse_command_line(argc, argv);
 
-    if (!check_generate) {
-        logreader.set_save_chek_messages(true);
-    }
-
-    set_signal_handlers();
-
-    // never use the system clock:
-    hal.scheduler->stop_clock(1);
-
-    hal.console->printf("Processing log %s\n", filename);
-
-    // remember filename for reporting
-    log_filename = filename;
-
-    if (!find_log_info(log_info)) {
-        printf("Update to get log information\n");
-        exit(1);
-    }
-
-    hal.console->printf("Using an update rate of %u Hz\n", log_info.update_rate);
-
-    if (!logreader.open_log(filename)) {
-        perror(filename);
-        exit(1);
-    }
-
     _vehicle.setup();
-
-    inhibit_gyro_cal();
-    force_log_disarmed();
-
-    // FIXME: base this on key parameters rather than the loop rate
-    if (log_info.update_rate == 400) {
-        // assume copter for 400Hz
-        _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
-        _vehicle.ahrs.set_fly_forward(false);
-    } else if (log_info.update_rate == 50) {
-        // assume copter for 400Hz
-        _vehicle.ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
-        _vehicle.ahrs.set_fly_forward(true);
-    }
-    
-    set_ins_update_rate(log_info.update_rate);
 }
 
-void Replay::set_ins_update_rate(uint16_t _update_rate) {
-    _vehicle.ins.init(_update_rate);
-}
-
-void Replay::inhibit_gyro_cal() {
-    if (!logreader.set_parameter("INS_GYR_CAL", AP_InertialSensor::GYRO_CAL_NEVER)) {
-        ::fprintf(stderr, "Failed to set GYR_CAL parameter\n");
-        abort();
-    }
-}
-void Replay::force_log_disarmed() {
-    if (!logreader.set_parameter("LOG_DISARMED", 1)) {
-        ::fprintf(stderr, "Failed to set LOG_DISARMED parameter\n");
-        abort();
-    }
-}
-
-/*
-  setup user -p parameters
- */
-void Replay::set_user_parameters(void)
+void Replay::loop()
 {
-    for (struct user_parameter *u=user_parameters; u; u=u->next) {
-        if (!logreader.set_parameter(u->name, u->value)) {
-            ::printf("Failed to set parameter %s to %f\n", u->name, u->value);
-            exit(1);
-        }
+    // LogReader reader = LogReader(log_structure);
+    if (!reader.open_log(filename)) {
+        ::fprintf(stderr, "open(%s): %m\n", filename);
+        exit(1);
     }
-}
-
-void Replay::set_signal_handlers(void)
-{
-    struct sigaction sa;
-
-    sigemptyset(&sa.sa_mask);
-    sa.sa_flags = 0;
-
-    if (generate_fpe) {
-        // SITL_State::_parse_command_line sets up an FPE handler.  We
-        // can do better:
-        feenableexcept(FE_INVALID | FE_OVERFLOW);
-        sa.sa_handler = _replay_sig_fpe;
-    } else {
-        // disable floating point exception generation:
-        int exceptions = FE_OVERFLOW | FE_DIVBYZERO;
-#ifndef __i386__
-        // i386 with gcc doesn't work with FE_INVALID
-        exceptions |= FE_INVALID;
-#endif
-        if (feclearexcept(exceptions)) {
-            ::fprintf(stderr, "Failed to disable floating point exceptions: %s", strerror(errno));
-        }
-        sa.sa_handler = SIG_IGN;
-    }
-
-    if (sigaction(SIGFPE, &sa, nullptr) < 0) {
-        ::fprintf(stderr, "Failed to set floating point exceptions' handler: %s", strerror(errno));
-    }
-}
-
-/*
-  write out EKF log messages
- */
-void Replay::write_ekf_logs(void)
-{
-    if (!LogReader::in_list("EKF", nottypes)) {
-        _vehicle.ahrs.Log_Write();
-    }
-    if (!LogReader::in_list("AHRS2", nottypes)) {
-        _vehicle.logger.Write_AHRS2();
-    }
-    if (!LogReader::in_list("POS", nottypes)) {
-        _vehicle.logger.Write_POS();
-    }
-}
-
-void Replay::read_sensors(const char *type, uint8_t core)
-{
-    if (streq(type, "PARM")) {
-        set_user_parameters();
-    }
-
-    if (!done_home_init) {
-        if (streq(type, "GPS") &&
-            (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) && done_baro_init) {
-            const Location &loc = _vehicle.gps.location();
-            ::printf("GPS Lock at %.7f %.7f %.2fm time=%.1f seconds\n", 
-                     loc.lat * 1.0e-7f, 
-                     loc.lng * 1.0e-7f,
-                     loc.alt * 0.01f,
-                     AP_HAL::millis()*0.001f);
-            if (!_vehicle.ahrs.set_home(loc)) {
-                ::printf("Failed to set home to that location!");
-            }
-            done_home_init = true;
-        }
-    }
-
-    if (streq(type,"GPS")) {
-        _vehicle.gps.update();
-        if (_vehicle.gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
-            _vehicle.ahrs.estimate_wind();
-        }
-    } else if (streq(type,"MAG")) {
-        _vehicle.compass.read();
-    } else if (streq(type,"ARSP")) {
-        _vehicle.ahrs.set_airspeed(&_vehicle.airspeed);
-    } else if (streq(type,"BARO")) {
-        _vehicle.barometer.update();
-        if (!done_baro_init) {
-            done_baro_init = true;
-            ::printf("Barometer initialised\n");
-            _vehicle.barometer.update_calibration();
-        }
-    } 
-
-    static bool ekf_force_started = false;
-    if (!ekf_force_started) {
-        if (log_info.have_imt2 ||
-            log_info.have_imt) {
-            _vehicle.ahrs.force_ekf_start();
-            ::fprintf(stderr, "EKF force-started at %" PRIu64 "\n", AP_HAL::micros64());
-            ekf_force_started = true;
-        }
-    }
-
-    bool run_ahrs = false;
-    if (log_info.have_imt2) {
-        run_ahrs = streq(type, "IMT2");
-    } else if (log_info.have_imt) {
-        run_ahrs = streq(type, "IMT");
-    } else if (log_info.have_imu2) {
-        run_ahrs = streq(type, "IMU2");
-    } else {
-        run_ahrs = streq(type, "IMU");
-    }
-
-    /*
-      always run AHRS on CHECK messages when checking the solution
-     */
-    if (check_solution) {
-        run_ahrs = streq(type, "CHEK");
-    }
-    
-    if (run_ahrs) {
-        _vehicle.ahrs.update();
-        if ((downsample == 0 || ++output_counter % downsample == 0) && !logmatch) {
-            write_ekf_logs();
-        }
-        if (_vehicle.ahrs.healthy() != ahrs_healthy) {
-            ahrs_healthy = _vehicle.ahrs.healthy();
-            printf("AHRS health: %u at %lu\n", 
-                   (unsigned)ahrs_healthy,
-                   (unsigned long)AP_HAL::millis());
-        }
-        if (check_generate) {
-            log_check_generate();
-        } else if (check_solution) {
-            log_check_solution();
-        }
-    }
-
-    // 255 here is a special marker for "no core present in log".
-    // This may give us a hope of backwards-compatability.
-    if (logmatch &&
-        (streq(type, "NKF1") || streq(type, "XKF1")) &&
-        (core == 0 || core == 255)) {
-        write_ekf_logs();
-    }
-}
-
-
-/*
-  copy current data to CHEK message
- */
-void Replay::log_check_generate(void)
-{
-    Vector3f euler;
-    Vector3f velocity;
-    Location loc {};
-
-    _vehicle.ahrs.EKF2.getEulerAngles(-1,euler);
-    _vehicle.ahrs.EKF2.getVelNED(-1,velocity);
-    _vehicle.ahrs.EKF2.getLLH(loc);
-
-    _vehicle.logger.Write(
-        "CHEK",
-        "TimeUS,Roll,Pitch,Yaw,Lat,Lng,Alt,VN,VE,VD",
-        "sdddDUmnnn",
-        "FBBBGGB000",
-        "QccCLLffff",
-        AP_HAL::micros64(),
-        (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg, displayed as deg due to format string)
-        (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg, displayed as deg due to format string)
-        (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg, displayed as deg due to format string)
-        loc.lat,
-        loc.lng,
-        loc.alt*0.01f,
-        velocity.x,
-        velocity.y,
-        velocity.z
-        );
-}
-
-
-/*
-  check current solution against CHEK message
- */
-void Replay::log_check_solution(void)
-{
-    const LR_MsgHandler::CheckState &check_state = logreader.get_check_state();
-    Vector3f euler;
-    Vector3f velocity;
-    Location loc {};
-
-    _vehicle.ahrs.EKF2.getEulerAngles(-1,euler);
-    _vehicle.ahrs.EKF2.getVelNED(-1,velocity);
-    _vehicle.ahrs.EKF2.getLLH(loc);
-
-    float roll_error  = degrees(fabsf(euler.x - check_state.euler.x));
-    float pitch_error = degrees(fabsf(euler.y - check_state.euler.y));
-    float yaw_error = wrap_180_cd(100*degrees(fabsf(euler.z - check_state.euler.z)))*0.01f;
-    float vel_error = (velocity - check_state.velocity).length();
-    float pos_error = check_state.pos.get_distance(loc);
-
-    check_result.max_roll_error  = MAX(check_result.max_roll_error,  roll_error);
-    check_result.max_pitch_error = MAX(check_result.max_pitch_error, pitch_error);
-    check_result.max_yaw_error   = MAX(check_result.max_yaw_error,   yaw_error);
-    check_result.max_vel_error   = MAX(check_result.max_vel_error,   vel_error);
-    check_result.max_pos_error   = MAX(check_result.max_pos_error,   pos_error);
-}
-
-void Replay::flush_and_exit()
-{
-    flush_logger();
-
-    if (check_solution) {
-        report_checks();
-    }
-
-    if (packet_counts) {
-        show_packet_counts();
+    while (reader.update()) {
     }
 
     // If we don't tear down the threads then they continue to access
@@ -830,196 +167,9 @@ void Replay::flush_and_exit()
     exit(0);
 }
 
-void Replay::show_packet_counts()
-{
-    uint64_t counts[LOGREADER_MAX_FORMATS];
-    logreader.get_packet_counts(counts);
-    char format_type[5];
-    uint64_t total = 0;
-    for(uint16_t i=0;i<LOGREADER_MAX_FORMATS;i++) {
-        if (counts[i] != 0) {
-            logreader.format_type(i, format_type);
-            printf("%10" PRIu64 " %s\n", counts[i], format_type);
-            total += counts[i];
-        }
-    }
+Replay replay(replayvehicle);
+AP_Vehicle& vehicle = replayvehicle;
 
-    printf("%" PRIu64 " total\n", total);
-}
-
-void Replay::loop()
-{
-    char type[5];
-    uint8_t core;
-
-    if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) {
-        if (!hal.util->get_soft_armed()) {
-            hal.util->set_soft_armed(true);
-            ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis());
-        }
-    }
-
-    if (!logreader.update(type, core)) {
-        ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f);
-        flush_and_exit();
-    }
-
-    const uint64_t now64 = AP_HAL::micros64();
-    if (last_timestamp != 0) {
-        if (now64 < last_timestamp) {
-            ::printf("time going backwards?! now=%" PRIu64 " last_timestamp=%" PRIu64 "us\n",
-                     now64, last_timestamp);
-            exit(1);
-        } else {
-            const uint64_t gap = now64 - last_timestamp;
-            if (gap > 40000) {
-                ::printf("Gap in log at timestamp=%" PRIu64 " of length %" PRIu64 "us\n",
-                         last_timestamp, gap);
-            }
-        }
-    }
-    last_timestamp = now64;
-
-    if (streq(type, "FMT")) {
-        if (!seen_non_fmt) {
-            return;
-        }
-    } else {
-        seen_non_fmt = true;
-    }
-
-    read_sensors(type, core);
-}
-
-
-bool Replay::show_error(const char *text, float max_error, float tolerance)
-{
-    bool failed = max_error > tolerance;
-    printf("%s:\t%.2f %c %.2f\n", 
-           text,
-           max_error,
-           failed?'>':'<',
-           tolerance);
-    return failed;
-}
-
-/*
-  report results of --check
- */
-void Replay::report_checks(void)
-{
-    bool failed = false;
-    if (tolerance_euler < 0.01f) {
-        tolerance_euler = 0.01f;
-    }
-    FILE *f = fopen("replay_results.txt","a");
-    if (f != NULL) {
-        fprintf(f, "%s\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n",
-                log_filename, 
-                check_result.max_roll_error,
-                check_result.max_pitch_error,
-                check_result.max_yaw_error,
-                check_result.max_pos_error,
-                check_result.max_vel_error);
-        fclose(f);
-    }
-    failed |= show_error("Roll error", check_result.max_roll_error, tolerance_euler);
-    failed |= show_error("Pitch error", check_result.max_pitch_error, tolerance_euler);
-    failed |= show_error("Yaw error", check_result.max_yaw_error, tolerance_euler);
-    failed |= show_error("Position error", check_result.max_pos_error, tolerance_pos);
-    failed |= show_error("Velocity error", check_result.max_vel_error, tolerance_vel);
-    if (failed) {
-        printf("Checks failed\n");
-        exit(1);
-    } else {
-        printf("Checks passed\n");
-    }
-}
-
-/*
-  parse a parameter file line
- */
-bool Replay::parse_param_line(char *line, char **vname, float &value)
-{
-    if (line[0] == '#') {
-        return false;
-    }
-    char *saveptr = NULL;
-    char *pname = strtok_r(line, ", =\t", &saveptr);
-    if (pname == NULL) {
-        return false;
-    }
-    if (strlen(pname) > AP_MAX_NAME_SIZE) {
-        return false;
-    }
-    const char *value_s = strtok_r(NULL, ", =\t", &saveptr);
-    if (value_s == NULL) {
-        return false;
-    }
-    value = atof(value_s);
-    *vname = pname;
-    return true;
-}
-
-
-/*
-  load a default set of parameters from a file
- */
-void Replay::load_param_file(const char *pfilename)
-{
-    FILE *f = fopen(pfilename, "r");
-    if (f == NULL) {
-        printf("Failed to open parameter file: %s\n", pfilename);
-        exit(1);
-    }
-    char line[100];
-
-    while (fgets(line, sizeof(line)-1, f)) {
-        char *pname;
-        float value;
-        if (!parse_param_line(line, &pname, value)) {
-            continue;
-        }
-        struct user_parameter *u = new user_parameter;
-        strncpy(u->name, pname, sizeof(u->name));
-        u->value = value;
-        u->next = user_parameters;
-        user_parameters = u;
-    }
-    fclose(f);
-}
-
-/*
-  see if a user parameter is set
- */
-bool Replay::check_user_param(const char *name)
-{
-    for (struct user_parameter *u=user_parameters; u; u=u->next) {
-        if (strcmp(name, u->name) == 0) {
-            return true;
-        }
-    }
-    return false;
-}
-
-const struct AP_Param::GroupInfo        GCS_MAVLINK_Parameters::var_info[] = {
-    AP_GROUPEND
-};
-GCS_Dummy _gcs;
-
-#include <AP_ADSB/AP_ADSB.h>
-#include <AP_Avoidance/AP_Avoidance.h>
-
-// dummy methods to avoid linking with these libraries
-bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }
-AP_AdvancedFailsafe *AP::advancedfailsafe() { return nullptr; }
-
-// dummy method to avoid linking AP_Avoidance
-AP_Avoidance *AP::ap_avoidance() { return nullptr; }
-
-// avoid building/linking LTM:
-void AP_LTM_Telem::init() {};
-// avoid building/linking Devo:
-void AP_DEVO_Telem::init() {};
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
 AP_HAL_MAIN_CALLBACKS(&replay);
diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h
index 1e0af1fdad..9732d03fec 100644
--- a/Tools/Replay/Replay.h
+++ b/Tools/Replay/Replay.h
@@ -13,41 +13,9 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-
-#include <AP_Common/AP_Common.h>
-#include <AP_Param/AP_Param.h>
-#include <StorageManager/StorageManager.h>
-#include <fenv.h>
-#include <AP_Math/AP_Math.h>
-#include <AP_HAL/AP_HAL.h>
-#include <AP_AccelCal/AP_AccelCal.h>
-#include <AP_Declination/AP_Declination.h>
-#include <Filter/Filter.h>
-#include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_Vehicle/AP_Vehicle.h>
-#include <AP_Notify/AP_Notify.h>
-#include <AP_Logger/AP_Logger.h>
-#include <GCS_MAVLink/GCS_MAVLink.h>
-#include <AP_GPS/AP_GPS.h>
-#include <AP_AHRS/AP_AHRS.h>
-#include <AP_Compass/AP_Compass.h>
-#include <AP_Baro/AP_Baro.h>
-#include <AP_InertialSensor/AP_InertialSensor.h>
-#include <AP_NavEKF2/AP_NavEKF2.h>
-#include <AP_NavEKF3/AP_NavEKF3.h>
-#include <AP_Mission/AP_Mission.h>
-#include <AP_Rally/AP_Rally.h>
-#include <AP_BattMonitor/AP_BattMonitor.h>
-#include <AP_Terrain/AP_Terrain.h>
-#include <AP_OpticalFlow/AP_OpticalFlow.h>
-#include <AP_SerialManager/AP_SerialManager.h>
-#include <RC_Channel/RC_Channel.h>
-#include <AP_RangeFinder/AP_RangeFinder.h>
-#include <stdio.h>
-#include <errno.h>
-#include <signal.h>
-#include <unistd.h>
-#include <AP_HAL/utility/getopt_cpp.h>
+
+#include "LogReader.h"
 
 class ReplayVehicle : public AP_Vehicle {
 public:
@@ -74,6 +42,9 @@ public:
     };
     AP_Logger logger{unused};
 
+    NavEKF2 ekf2;
+    NavEKF3 ekf3;
+
 protected:
 
     void init_ardupilot() override;
@@ -87,133 +58,22 @@ private:
     static const AP_Param::Info var_info[];
 };
 
-
 class Replay : public AP_HAL::HAL::Callbacks {
+
 public:
     Replay(ReplayVehicle &vehicle) :
-        filename("log.bin"),
         _vehicle(vehicle) { }
 
-    // HAL::Callbacks implementation.
     void setup() override;
     void loop() override;
 
-    void flush_logger(void);
-    void show_packet_counts();
-
-    bool check_solution = false;
-    const char *log_filename = NULL;
-    bool generate_fpe = true;
-
-    /*
-      information about a log from find_log_info
-     */
-    struct log_information {
-        uint16_t update_rate;
-        bool have_imu2:1;
-        bool have_imt:1;
-        bool have_imt2:1;
-    } log_info {};
-
-    // return true if a user parameter of name is set
-    bool check_user_param(const char *name);
-    
 private:
     const char *filename;
     ReplayVehicle &_vehicle;
 
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    SITL::SITL sitl;
-#endif
-
-    LogReader logreader{_vehicle.ahrs,
-            _vehicle.ins,
-            _vehicle.compass,
-            _vehicle.gps,
-            _vehicle.airspeed,
-            _vehicle.logger,
-            _vehicle.log_structure,
-            0,
-            nottypes};
-
-    FILE *ekf1f;
-    FILE *ekf2f;
-    FILE *ekf3f;
-    FILE *ekf4f;
-
-    bool done_baro_init;
-    bool done_home_init;
-    int32_t arm_time_ms = -1;
-    bool ahrs_healthy;
-    bool use_imt = true;
-    bool check_generate = false;
-    float tolerance_euler = 3;
-    float tolerance_pos = 2;
-    float tolerance_vel = 2;
-    const char **nottypes = NULL;
-    uint16_t downsample = 0;
-    bool logmatch = false;
-    uint32_t output_counter = 0;
-    uint64_t last_timestamp = 0;
-    bool packet_counts = false;
-
-    struct {
-        float max_roll_error;
-        float max_pitch_error;
-        float max_yaw_error;
-        float max_pos_error;
-        float max_alt_error;
-        float max_vel_error;
-    } check_result {};
+    LogReader reader{_vehicle.log_structure, _vehicle.ekf2, _vehicle.ekf3};
 
     void _parse_command_line(uint8_t argc, char * const argv[]);
 
-    struct user_parameter {
-        struct user_parameter *next;
-        char name[17];
-        float value;
-    } *user_parameters;
 
-    void set_ins_update_rate(uint16_t update_rate);
-    void inhibit_gyro_cal();
-    void force_log_disarmed();
-
-    void usage(void);
-    void set_user_parameters(void);
-    void read_sensors(const char *type, uint8_t core);
-    void write_ekf_logs(void);
-    void log_check_generate();
-    void log_check_solution();
-    bool show_error(const char *text, float max_error, float tolerance);
-    void report_checks();
-    bool find_log_info(struct log_information &info);
-    const char **parse_list_from_string(const char *str);
-    bool parse_param_line(char *line, char **vname, float &value);
-    void load_param_file(const char *filename);
-    void set_signal_handlers(void);
-    void flush_and_exit();
-
-    FILE *xfopen(const char *f, const char *mode);
-
-    bool seen_non_fmt;
 };
-
-/*
-  Replay specific log structures
- */
-struct PACKED log_Chek {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    int16_t roll;
-    int16_t pitch;
-    uint16_t yaw;
-    int32_t lat;
-    int32_t lng;
-    float alt;
-    float vnorth;
-    float veast;
-    float vdown;
-};
-
-
-extern Replay replay;
diff --git a/Tools/Replay/check_replay.py b/Tools/Replay/check_replay.py
new file mode 100755
index 0000000000..2e8fda9b21
--- /dev/null
+++ b/Tools/Replay/check_replay.py
@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+
+'''
+check that replay produced identical results
+'''
+
+import time
+
+from argparse import ArgumentParser
+parser = ArgumentParser(description=__doc__)
+parser.add_argument("--condition", default=None, help="condition for packets")
+parser.add_argument("--ekf2-only", action='store_true', help="only check EKF2")
+parser.add_argument("--ekf3-only", action='store_true', help="only check EKF3")
+parser.add_argument("--verbose", action='store_true', help="verbose output")
+parser.add_argument("logs", metavar="LOG", nargs="+")
+
+args = parser.parse_args()
+
+from pymavlink import mavutil
+
+
+def check_log(logfile):
+    '''check replay log for matching output'''
+    print("Processing log %s" % filename)
+    count = 0
+    errors = 0
+    counts = {}
+
+    mlog = mavutil.mavlink_connection(filename)
+
+    ek2_list = ['NKF1','NKF2','NKF3','NKF4','NKF5','NKF0','NKQ']
+    ek3_list = ['XKF1','XKF2','XKF3','XKF4','XKF0','XKFS','XKQ','XKFD','XKV1','XKV2']
+    
+    if args.ekf2_only:
+        mlist = ek2_list
+    elif args.ekf3_only:
+        mlist = ek3_list
+    else:
+        mlist = ek2_list + ek3_list
+
+    base = {}
+    for m in mlist:
+        base[m] = {}
+
+    while True:
+        m = mlog.recv_match(type=mlist)
+        if m is None:
+            break
+        if not hasattr(m,'C'):
+            continue
+        mtype = m.get_type()
+        core = m.C
+        if core < 100:
+            base[mtype][core] = m
+            continue
+        mb = base[mtype][core-100]
+        count += 1
+        if not mtype in counts:
+            counts[mtype] = 0
+        counts[mtype] += 1
+        mismatch = False
+        for f in m._fieldnames:
+            if f == 'C':
+                continue
+            v1 = getattr(m,f)
+            v2 = getattr(mb,f)
+            if v1 != v2:
+                mismatch = True
+                errors += 1
+                print("Mismatch in field %s.%s: %s %s" % (mtype, f, str(v1), str(v2)))
+        if mismatch:
+            print(mb)
+            print(m)
+    print("Processed %u messages, %u errors" % (count, errors))
+    if args.verbose:
+        print(counts)
+
+for filename in args.logs:
+    check_log(filename)