I am going to use the EKF filter in the ardupilot navigation code for keeping track of position of a rover, where the IMU and GPS are not connected directly to the ardupilot code, but comes by another route. The GPS is also not a GPS, but comes from an indoor positioning system. Our rover is a very loosely put together thing, with sensors and cables and processors, not using one of the standard controllers like pixhawk. In order to debug and test the EKF, I am feeding it recorded input which I read from a file instead of what AP_InertialSensor_SITL ordinarily does, currently at the frequency 500Hz, because that is the speed at which I’m reading IMU data from a MPU9150. I’m leaving out the GPS for the time being.
The positions that the ardupilot code computes (and which I output to stdout in AP_NavEKF_core) seem to have very little to do with the input. When I do a simple calculation myself in a separate program, I see the actual movement being traced reliably for at least 10 seconds. My input contains at least 10s of standing still, and calibration of the IMU code seems to be done correctly.
Has someone done this before (feeding recorded input into ardupilot) and solved similar problems? What could I be doing wrong? I want this to be a standalone program, so I have no ground control.
I attach my main program. It started with the example GPS_AUTO_test.cpp, and I added some things. Strange-looking constructs in the beginning are probably because I didn’t understand the parameter concept yet.
// -- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil --
//
// Test for AP_GPS_AUTO
//
#include <stdlib.h>
#include <stdio.h>
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_GPS/AP_GPS.h>
#include <DataFlash/DataFlash.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_ADC/AP_ADC.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Baro/AP_Baro.h>
#include <Filter/Filter.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Declination/AP_Declination.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_Mission/AP_Mission.h>
#include <StorageManager/StorageManager.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_Math/AP_Math.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_Notify/AP_BoardLED.h>
#include <AP_NavEKF/AP_NavEKF.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
#include <SITL/SITL.h>
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, k_param_ ## v, &apan.v, {group_info:class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, k_param_ ## pname, &apan.v, {group_info:class::var_info} }
#define k_param_sitl 1
#define k_param_gps 2
#define k_param_compass 2
#define k_param_NavEKF2 2
class apa {
public:
// create board led object
//AP_BoardLED board_led;
Compass compass;
SITL::SITL sitl;
AP_GPS gps;
// This example uses GPS system. Create it.
//AP_GPS gps;
// Serial manager is needed for UART comunications
AP_SerialManager serial_manager;
AP_Baro barometer;
AP_InertialSensor ins;
RangeFinder sonar { serial_manager };
NavEKF EKF{&ahrs, barometer, sonar};
NavEKF2 EKF2{&ahrs, barometer, sonar};
AP_AHRS_NavEKF ahrs {ins, barometer, gps, sonar, EKF, EKF2};
//AP_AHRS_DCM ahrs {ins, barometer, gps};
static const AP_Param::Info var_info[];
};
apa apan;
const AP_Param::Info apa::var_info[] = {
// @Group: SIM_
// @Path: …/…/…/libraries/SITL/SITL.cpp
GOBJECT(sitl, “SIM_”, SITL::SITL),
// GPS driver
// @Group: GPS_
// @Path: …/libraries/AP_GPS/AP_GPS.cpp
GOBJECT(gps, “GPS_”, AP_GPS),
// @Group: COMPASS_
// @Path: …/libraries/AP_Compass/Compass.cpp
GOBJECT(compass, “COMPASS_”, Compass),
// // @Group: EK2_
// // @Path: …/libraries/AP_NavEKF2/AP_NavEKF2.cpp
// GOBJECTN(EKF2, NavEKF2, “EK2_”, NavEKF2),
AP_VAREND
};
AP_Param param_loader(apan.var_info);
#define T6 1000000
#define T7 10000000
void setup()
{
hal.console->println(“GPS AUTO library test”);
// Initialise the leds
//apan.board_led.init();
// Initialize the UART for GPS system
apan.serial_manager.init();
apan.gps.init(NULL, apan.serial_manager);
apan.ins.init(AP_InertialSensor::RATE_100HZ);
apan.compass.init();
apan.ahrs.set_compass(&apan.compass);
}
void loop()
{
static uint32_t last_msg_ms;
apan.ahrs.update();
// Update GPS state based on possible bytes received from the module.
apan.gps.update();
// If new GPS data is received, output it's contents to the console
// Here we rely on the time of the message in GPS class and the time of last message
// saved in static variable last_msg_ms. When new message is received, the time
// in GPS class will be updated.
if (last_msg_ms != apan.gps.last_message_time_ms()) {
// Reset the time of message
last_msg_ms = apan.gps.last_message_time_ms();
// Acquire location
const Location &loc = apan.gps.location();
#if 0
//printf(“loop %d\n”, last_msg_ms);
printf(“GPS lat, lng = %d %d\n”, loc.lat, loc.lng);
#endif
#if 0
// Print the contents of message
hal.console->print(“Lat: “);
print_latlon(hal.console, loc.lat);
hal.console->print(” Lon: “);
print_latlon(hal.console, loc.lng);
hal.console->printf(” Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %u/%lu STATUS: %u\n”,
loc.alt * 0.01f,
apan.gps.ground_speed(),
(int)apan.gps.ground_course_cd() / 100,
apan.gps.num_sats(),
apan.gps.time_week(),
(unsigned long)apan.gps.time_week_ms(),
apan.gps.status());
#endif
}
// Delay for 10 mS will give us 100 Hz invocation rate
hal.scheduler->delay(10);
}
// Register above functions in HAL board level
AP_HAL_MAIN();