diff --git a/libraries/SITL/SIM_VectorNav.cpp b/libraries/SITL/SIM_VectorNav.cpp
index ed4c1bcaedd53d..44499377d48b8e 100644
--- a/libraries/SITL/SIM_VectorNav.cpp
+++ b/libraries/SITL/SIM_VectorNav.cpp
@@ -29,37 +29,49 @@ VectorNav::VectorNav() :
 {
 }
 
-struct PACKED VN_INS_packet1 {
-    float uncompMag[3];
+
+struct PACKED VN_IMU_packet_sim {
+    static constexpr uint8_t header[]{0x01, 0x21, 0x07};
+    uint64_t timeStartup;
+    float gyro[3];
+    float accel[3];
     float uncompAccel[3];
     float uncompAngRate[3];
-    float pressure;
     float mag[3];
-    float accel[3];
-    float gyro[3];
+    float temp;
+    float pressure;
+};
+constexpr uint8_t VN_IMU_packet_sim::header[];
+
+struct PACKED VN_INS_ekf_packet_sim {
+    static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06};
+    uint64_t timeStartup;
     float ypr[3];
     float quaternion[4];
     float yprU[3];
-    double positionLLA[3];
-    float velNED[3];
+    uint16_t insStatus;
+    double posLla[3];
+    float velNed[3];
     float posU;
     float velU;
 };
-
-struct PACKED VN_INS_packet2 {
-    uint64_t timeGPS;
-    float temp;
-    uint8_t numGPS1Sats;
-    uint8_t GPS1Fix;
-    double GPS1posLLA[3];
-    float GPS1velNED[3];
-    float GPS1DOP[7];
-    uint8_t numGPS2Sats;
-    uint8_t GPS2Fix;
+constexpr uint8_t VN_INS_ekf_packet_sim::header[];
+
+struct PACKED VN_INS_gnss_packet_sim {
+    static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00};
+    uint64_t timeStartup;
+    uint64_t timeGps;
+    uint8_t numSats1;
+    uint8_t fix1;
+    double posLla1[3];
+    float velNed1[3];
+    float posU1[3];
+    float velU1;
+    float dop1[7];
+    uint8_t numSats2;
+    uint8_t fix2;
 };
-
-#define VN_PKT1_HEADER { 0xFA, 0x34, 0x2E, 0x07, 0x06, 0x01, 0x12, 0x06 }
-#define VN_PKT2_HEADER { 0xFA, 0x4E, 0x02, 0x00, 0x10, 0x00, 0xB8, 0x20, 0x18, 0x00 }
+constexpr uint8_t VN_INS_gnss_packet_sim::header[];
 
 /*
   get timeval using simulation time
@@ -80,36 +92,62 @@ static void simulation_timeval(struct timeval *tv)
     tv->tv_usec = new_usec % 1000000ULL;
 }
 
-void VectorNav::send_packet1(void)
+void VectorNav::send_imu_packet(void)
 {
     const auto &fdm = _sitl->state;
 
-    struct VN_INS_packet1 pkt {};
+    struct VN_IMU_packet_sim pkt {};
+
+    pkt.timeStartup = AP_HAL::micros() * 1e3;
+    
+    
+    const float gyro_noise = 0.05;
+
+    pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise);
+    pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise);
+    pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise);
+    
+    pkt.accel[0] = fdm.xAccel;
+    pkt.accel[1] = fdm.yAccel;
+    pkt.accel[2] = fdm.zAccel;
 
     pkt.uncompAccel[0] = fdm.xAccel;
     pkt.uncompAccel[1] = fdm.yAccel;
     pkt.uncompAccel[2] = fdm.zAccel;
-    const float gyro_noise = 0.05;
+    
     pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float());
     pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float());
     pkt.uncompAngRate[2] = radians(fdm.yawRate + gyro_noise * rand_float());
 
-    const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
-    pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01;
-
     pkt.mag[0] = fdm.bodyMagField.x*0.001;
     pkt.mag[1] = fdm.bodyMagField.y*0.001;
     pkt.mag[2] = fdm.bodyMagField.z*0.001;
-    pkt.uncompMag[0] = pkt.mag[0];
-    pkt.uncompMag[1] = pkt.mag[1];
-    pkt.uncompMag[2] = pkt.mag[2];
 
-    pkt.accel[0] = fdm.xAccel;
-    pkt.accel[1] = fdm.yAccel;
-    pkt.accel[2] = fdm.zAccel;
-    pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise);
-    pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise);
-    pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise);
+    pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude);
+
+    const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
+    pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01;
+
+    const uint8_t sync_byte = 0xFA;
+    write_to_autopilot((char *)sync_byte, 1);
+    write_to_autopilot((char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header));
+    write_to_autopilot((char *)&pkt, sizeof(pkt));
+
+    uint16_t crc = crc16_ccitt(&VN_IMU_packet_sim::header[0], sizeof(VN_IMU_packet_sim::header), 0);
+    crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
+    uint16_t crc2;
+    swab(&crc, &crc2, 2);
+
+    write_to_autopilot((char *)&crc2, sizeof(crc2));
+}
+
+void VectorNav::send_ins_ekf_packet(void)
+{
+    const auto &fdm = _sitl->state;
+
+    struct VN_INS_ekf_packet_sim pkt {};
+
+    pkt.timeStartup = AP_HAL::micros() * 1e3;
 
     pkt.ypr[0] = fdm.yawDeg;
     pkt.ypr[1] = fdm.pitchDeg;
@@ -120,58 +158,74 @@ void VectorNav::send_packet1(void)
     pkt.quaternion[2] = fdm.quaternion.q4;
     pkt.quaternion[3] = fdm.quaternion.q1;
 
-    pkt.positionLLA[0] = fdm.latitude;
-    pkt.positionLLA[1] = fdm.longitude;
-    pkt.positionLLA[2] = fdm.altitude;
-    pkt.velNED[0] = fdm.speedN;
-    pkt.velNED[1] = fdm.speedE;
-    pkt.velNED[2] = fdm.speedD;
+    pkt.yprU[0] = 0.03;
+    pkt.yprU[1] = 0.03;
+    pkt.yprU[2] = 0.15;
+
+    pkt.insStatus = 0x0306;
+
+    pkt.posLla[0] = fdm.latitude;
+    pkt.posLla[1] = fdm.longitude;
+    pkt.posLla[2] = fdm.altitude;
+    pkt.velNed[0] = fdm.speedN;
+    pkt.velNed[1] = fdm.speedE;
+    pkt.velNed[2] = fdm.speedD;
     pkt.posU = 0.5;
     pkt.velU = 0.25;
 
-    const uint8_t header[] VN_PKT1_HEADER;
-
-    write_to_autopilot((char *)&header, sizeof(header));
+    const uint8_t sync_byte = 0xFA;
+    write_to_autopilot((char *)sync_byte, 1);
+    write_to_autopilot((char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header));
     write_to_autopilot((char *)&pkt, sizeof(pkt));
 
-    uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0);
+    uint16_t crc = crc16_ccitt(&VN_INS_ekf_packet_sim::header[0], sizeof(VN_INS_ekf_packet_sim::header), 0);
     crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
+
     uint16_t crc2;
     swab(&crc, &crc2, 2);
 
     write_to_autopilot((char *)&crc2, sizeof(crc2));
 }
 
-void VectorNav::send_packet2(void)
+void VectorNav::send_ins_gnss_packet(void)
 {
     const auto &fdm = _sitl->state;
 
-    struct VN_INS_packet2 pkt {};
+    struct VN_INS_gnss_packet_sim pkt {};
+
+    pkt.timeStartup = AP_HAL::micros() * 1e3;
 
     struct timeval tv;
     simulation_timeval(&tv);
 
-    pkt.timeGPS = tv.tv_usec * 1000ULL;
-
-    pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude);
-    pkt.numGPS1Sats = 19;
-    pkt.GPS1Fix = 3;
-    pkt.GPS1posLLA[0] = fdm.latitude;
-    pkt.GPS1posLLA[1] = fdm.longitude;
-    pkt.GPS1posLLA[2] = fdm.altitude;
-    pkt.GPS1velNED[0] = fdm.speedN;
-    pkt.GPS1velNED[1] = fdm.speedE;
-    pkt.GPS1velNED[2] = fdm.speedD;
-    // pkt.GPS1DOP =
-    pkt.numGPS2Sats = 18;
-    pkt.GPS2Fix = 3;
-
-    const uint8_t header[] VN_PKT2_HEADER;
-
-    write_to_autopilot((char *)&header, sizeof(header));
+    pkt.timeGps = tv.tv_usec * 1000ULL;
+
+    pkt.numSats1 = 19;
+    pkt.fix1 = 3;
+    pkt.posLla1[0] = fdm.latitude;
+    pkt.posLla1[1] = fdm.longitude;
+    pkt.posLla1[2] = fdm.altitude;
+    pkt.velNed1[0] = fdm.speedN;
+    pkt.velNed1[1] = fdm.speedE;
+    pkt.velNed1[2] = fdm.speedD;
+
+    pkt.posU1[0] = 1;
+    pkt.posU1[0] = 1;
+    pkt.posU1[0] = 1.5;
+
+    pkt.velNed1[0] = 0.05;
+    pkt.velNed1[0] = 0.05;
+    pkt.velNed1[0] = 0.05;
+    // pkt.dop1 =
+    pkt.numSats2 = 18;
+    pkt.fix2 = 3;
+
+    const uint8_t sync_byte = 0xFA;
+    write_to_autopilot((char *)sync_byte, 1);
+    write_to_autopilot((char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header));
     write_to_autopilot((char *)&pkt, sizeof(pkt));
 
-    uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0);
+    uint16_t crc = crc16_ccitt(&VN_INS_gnss_packet_sim::header[0], sizeof(VN_INS_gnss_packet_sim::header), 0);
     crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
 
     uint16_t crc2;
@@ -203,14 +257,19 @@ void VectorNav::update(void)
     }
 
     uint32_t now = AP_HAL::micros();
-    if (now - last_pkt1_us >= 20000) {
-        last_pkt1_us = now;
-        send_packet1();
+    if (now - last_imu_pkt_us >= 20000) {
+        last_imu_pkt_us = now;
+        send_imu_packet();
     }
-
-    if (now - last_pkt2_us >= 200000) {
-        last_pkt2_us = now;
-        send_packet2();
+    
+    if (now - last_ekf_pkt_us >= 20000) {
+        last_ekf_pkt_us = now;
+        send_ins_ekf_packet();
+    }
+    
+    if (now - last_gnss_pkt_us >= 200000) {
+        last_gnss_pkt_us = now;
+        send_ins_gnss_packet();
     }
 
     char receive_buf[50];
diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h
index 589539e5d9a8f3..77fde25625733a 100644
--- a/libraries/SITL/SIM_VectorNav.h
+++ b/libraries/SITL/SIM_VectorNav.h
@@ -41,12 +41,13 @@ class VectorNav : public SerialDevice {
     void update(void);
 
 private:
-    uint32_t last_pkt1_us;
-    uint32_t last_pkt2_us;
-    uint32_t last_type_us;
+    uint32_t last_imu_pkt_us;
+    uint32_t last_ekf_pkt_us;
+    uint32_t last_gnss_pkt_us;
 
-    void send_packet1();
-    void send_packet2();
+    void send_imu_packet();
+    void send_ins_ekf_packet();
+    void send_ins_gnss_packet();
     void nmea_printf(const char *fmt, ...);
 };