void ahrs_update_mag_full(void) {
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, ahrs_impl.mag_h);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual_imu;
FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
// DISPLAY_FLOAT_VECT3("# expected", expected_imu);
// DISPLAY_FLOAT_VECT3("# measured", measured_imu);
// DISPLAY_FLOAT_VECT3("# residual", residual);
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
*/
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
static void send_mag(struct transport_tx *trans, struct link_device *dev)
{
struct FloatVect3 mag_float;
MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
pprz_msg_send_IMU_MAG(trans, dev, AC_ID,
&mag_float.x, &mag_float.y, &mag_float.z);
}
void ahrs_fc_update_mag_2d_dumb(struct Int32Vect3 *mag)
{
/* project mag on local tangeant plane */
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_rmat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat);
struct FloatVect3 magf;
MAGS_FLOAT_OF_BFP(magf, *mag);
const float cphi = cosf(ltp_to_imu_euler.phi);
const float sphi = sinf(ltp_to_imu_euler.phi);
const float ctheta = cosf(ltp_to_imu_euler.theta);
const float stheta = sinf(ltp_to_imu_euler.theta);
const float mn = ctheta * magf.x + sphi * stheta * magf.y + cphi * stheta * magf.z;
const float me = 0. * magf.x + cphi * magf.y - sphi * magf.z;
const float res_norm = -RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 0) * me +
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn;
const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 1),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
};
const float mag_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
const float mag_bias_update_gain = -2.5e-4;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm));
}
void ahrs_fc_update_mag_full(struct Int32Vect3 *mag, float dt)
{
struct FloatVect3 expected_imu;
float_rmat_vmult(&expected_imu, &ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.mag_h);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, *mag);
struct FloatVect3 residual_imu;
VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
// DISPLAY_FLOAT_VECT3("# expected", expected_imu);
// DISPLAY_FLOAT_VECT3("# measured", measured_imu);
// DISPLAY_FLOAT_VECT3("# residual", residual);
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt
* with ahrs_fc.mag_cnt beeing the number of propagations since last update
*/
const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2 * dt
*/
const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_mag(void) {
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
const float h_noise[] = { 0.1610, 0.1771, 0.2659};
update_state(&ahrs_impl.mag_h, &imu_h, h_noise);
reset_state();
}
static void dialog_with_io_proc() {
struct AutopilotMessageCRCFrame msg_in;
struct AutopilotMessageCRCFrame msg_out;
uint8_t crc_valid;
for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
// for (uint8_t i=0; i<4; i++) msg_out.payload.msg_down.csc_servo_cmd[i] = otp.csc_servo_outputs[i];
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up;
RATES_FLOAT_OF_BFP(otp.imu.gyro, in->gyro);
ACCELS_FLOAT_OF_BFP(otp.imu.accel, in->accel);
MAGS_FLOAT_OF_BFP(otp.imu.mag, in->mag);
otp.io_proc_msg_cnt = in->stm_msg_cnt;
otp.io_proc_err_cnt = in->stm_crc_err_cnt;
otp.rc_status = in->rc_status;
otp.baro_abs = in->pressure_absolute;
otp.baro_diff = in->pressure_differential;
}
void ahrs_update_mag_2d(void) {
const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 measured_ltp;
FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu);
const struct FloatVect3 residual_ltp =
{ 0,
0,
measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };
// printf("res : %f\n", residual_ltp.z);
struct FloatVect3 residual_imu;
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
const float mag_bias_update_gain = -2.5e-3;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
void print_raw_log_entry(struct raw_log_entry* e){
struct DoubleVect3 tempvect;
struct DoubleRates temprates;
printf("%f\t", e->time);
printf("%i\t", e->message.valid_sensors);
RATES_FLOAT_OF_BFP(temprates, e->message.gyro);
printf("% f % f % f\t", temprates.p, temprates.q, temprates.r);
ACCELS_FLOAT_OF_BFP(tempvect, e->message.accel);
printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z);
MAGS_FLOAT_OF_BFP(tempvect, e->message.mag);
printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z);
printf("% i % i % i\t", e->message.ecef_pos.x, e->message.ecef_pos.y, e->message.ecef_pos.z);
printf("% i % i % i\t", e->message.ecef_vel.x, e->message.ecef_vel.y, e->message.ecef_vel.z);
double baro_scaled = (double)(e->message.pressure_absolute)/256;
printf("%f", baro_scaled);
}
void ahrs_fc_update_mag_2d(struct Int32Vect3 *mag, float dt)
{
struct FloatVect2 expected_ltp;
VECT2_COPY(expected_ltp, ahrs_fc.mag_h);
// normalize expected ltp in 2D (x,y)
float_vect2_normalize(&expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, *mag);
struct FloatVect3 measured_ltp;
float_rmat_transp_vmult(&measured_ltp, &ahrs_fc.ltp_to_imu_rmat, &measured_imu);
struct FloatVect2 measured_ltp_2d = {measured_ltp.x, measured_ltp.y};
// normalize measured ltp in 2D (x,y)
float_vect2_normalize(&measured_ltp_2d);
struct FloatVect3 residual_ltp = {
0,
0,
measured_ltp_2d.x *expected_ltp.y - measured_ltp_2d.y * expected_ltp.x
};
// printf("res : %f\n", residual_ltp.z);
struct FloatVect3 residual_imu;
float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * ahrs_fc.mag_cnt
* with ahrs_fc.mag_cnt beeing the number of propagations since last update
*/
const float mag_rate_update_gain = 2 * ahrs_fc.mag_zeta * ahrs_fc.mag_omega * ahrs_fc.mag_cnt;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2 * dt
*/
const float mag_bias_update_gain = -(ahrs_fc.mag_omega * ahrs_fc.mag_omega) * dt;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_mag_2d(void) {
struct FloatVect2 expected_ltp;
VECT2_COPY(expected_ltp, ahrs_impl.mag_h);
// normalize expected ltp in 2D (x,y)
FLOAT_VECT2_NORMALIZE(expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 measured_ltp;
FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu);
struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y};
// normalize measured ltp in 2D (x,y)
FLOAT_VECT2_NORMALIZE(measured_ltp_2d);
const struct FloatVect3 residual_ltp =
{ 0,
0,
measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x };
// printf("res : %f\n", residual_ltp.z);
struct FloatVect3 residual_imu;
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
*/
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_mag_full_2d_dumb(void) {
/* project mag on local tangeant plane */
struct FloatVect3 magf;
MAGS_FLOAT_OF_BFP(magf, imu.mag);
const float cphi = cosf(ahrs_float.ltp_to_imu_euler.phi);
const float sphi = sinf(ahrs_float.ltp_to_imu_euler.phi);
const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta);
const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta);
const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm));
const float mag_bias_update_gain = -2.5e-4;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm));
}
void ahrs_update_mag_full(void) {
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, ahrs_impl.mag_h);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual_imu;
FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
// DISPLAY_FLOAT_VECT3("# expected", expected_imu);
// DISPLAY_FLOAT_VECT3("# measured", measured_imu);
// DISPLAY_FLOAT_VECT3("# residual", residual);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
const float mag_bias_update_gain = -2.5e-3;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_mag2(void) {
const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual;
FLOAT_VECT3_CROSS_PRODUCT(residual, measured_imu, expected_imu);
// FLOAT_VECT3_DIFF(residual, expected_imu, measured_imu);
DISPLAY_FLOAT_VECT3(" expected", expected_imu);
DISPLAY_FLOAT_VECT3(" measured", measured_imu);
DISPLAY_FLOAT_VECT3("residual", residual);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, mag_rate_update_gain);
}
void ahrs_update_mag(void) {
/* project mag on local tangeant plane */
struct FloatVect3 magf;
MAGS_FLOAT_OF_BFP(magf, imu.mag);
const float cphi = cosf(ahrs_float.ltp_to_imu_euler.phi);
const float sphi = sinf(ahrs_float.ltp_to_imu_euler.phi);
const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta);
const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta);
const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
printf("res norm %f\n", res_norm);
struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
DISPLAY_FLOAT_VECT3("r2", (*r2));
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm));
DISPLAY_FLOAT_RATES("corr", ahrs_impl.rate_correction);
const float mag_bias_update_gain = -2.5e-4;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm));
/* FIXME: saturate bias */
}
static void send_mag(void) {
struct FloatVect3 mag_float;
MAGS_FLOAT_OF_BFP(mag_float, imu.mag);
DOWNLINK_SEND_IMU_MAG(DefaultChannel, DefaultDevice,
&mag_float.x, &mag_float.y, &mag_float.z);
}
static struct raw_log_entry first_entry_after_initialisation(int file_descriptor){
int imu_measurements = 0, // => Gyro + Accel
magnetometer_measurements = 0,
baro_measurements = 0,
gps_measurements = 0; // only the position
struct DoubleMat33 attitude_profile_matrix, sigmaB; // the attitude profile matrix is often called "B"
struct Orientation_Measurement gravity,
magneto,
fake;
struct DoubleQuat q_ned2body, sigma_q;
/* Prepare the attitude profile matrix */
FLOAT_MAT33_ZERO(attitude_profile_matrix);
FLOAT_MAT33_ZERO(sigmaB);
// for faster converging, but probably more rounding error
#define MEASUREMENT_WEIGHT_SCALE 10
/* set the gravity measurement */
VECT3_ASSIGN(gravity.reference_direction, 0,0,-1);
gravity.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(imu_frequency); // originally 1/(imu_frequency*gravity.norm()
//gravity.weight_of_the_measurement = 1;
/* set the magneto - measurement */
EARTHS_GEOMAGNETIC_FIELD_NORMED(magneto.reference_direction);
magneto.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(mag_frequency); // originally 1/(mag_frequency*reference_direction.norm()
//magneto.weight_of_the_measurement = 1;
uint8_t read_ok;
#if WITH_GPS
struct raw_log_entry e = next_GPS(file_descriptor);
#else /* WITH_GPS */
struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
pos_0_ecef = Vector3d(4627578.56, 119659.25, 4373248.00);
pos_cov_0 = Vector3d::Ones()*100;
speed_0_ecef = Vector3d::Zero();
speed_cov_0 = Vector3d::Ones();
#endif /* WITH_GPS */
#ifdef EKNAV_FROM_LOG_DEBUG
int imu_ready = 0,
mag_ready = 0,
baro_ready = 0,
gps_ready = 0;
#endif /* EKNAV_FROM_LOG_DEBUG */
for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements); e = read_raw_log_entry(file_descriptor, &read_ok)){
if(IMU_READY(e.message.valid_sensors)){
imu_measurements++;
// update the estimated bias
bias_0 = NEW_MEAN(bias_0, RATES_BFP_AS_VECTOR3D(e.message.gyro), imu_measurements);
// update the attitude profile matrix
ACCELS_FLOAT_OF_BFP(gravity.measured_direction,e.message.accel);
add_orientation_measurement(&attitude_profile_matrix, gravity);
}
if(MAG_READY(e.message.valid_sensors)){
magnetometer_measurements++;
// update the attitude profile matrix
MAGS_FLOAT_OF_BFP(magneto.measured_direction,e.message.mag);
add_orientation_measurement(&attitude_profile_matrix, magneto);
// now, generate fake measurement with the last gravity measurement
fake = fake_orientation_measurement(gravity, magneto);
add_orientation_measurement(&attitude_profile_matrix, fake);
}
if(BARO_READY(e.message.valid_sensors)){
baro_measurements++;
// TODO: Fix it!
//NEW_MEAN(baro_0_height, BARO_FLOAT_OF_BFP(e.message.pressure_absolute), baro_measurements);
baro_0_height = (baro_0_height*(baro_measurements-1)+BARO_FLOAT_OF_BFP(e.message.pressure_absolute))/baro_measurements;
}
if(GPS_READY(e.message.valid_sensors)){
gps_measurements++;
// update the estimated bias
pos_0_ecef = NEW_MEAN(pos_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_pos)/100, gps_measurements);
speed_0_ecef = NEW_MEAN(speed_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_vel)/100, gps_measurements);
}
#ifdef EKNAV_FROM_LOG_DEBUG
if(imu_ready==0){
if(!NOT_ENOUGH_IMU_MEASUREMENTS(imu_measurements)){
printf("IMU READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
imu_ready = 1;
}
}
if(mag_ready==0){
if(!NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(magnetometer_measurements)){
printf("MAG READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
mag_ready = 1;
}
}
if(baro_ready==0){
if(!NOT_ENOUGH_BARO_MEASUREMENTS(baro_measurements)){
printf("BARO READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
baro_ready = 1;
}
}
if(gps_ready==0){
if(!NOT_ENOUGH_GPS_MEASUREMENTS(gps_measurements)){
printf("GPS READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
gps_ready = 1;
}
}
#endif /* EKNAV_FROM_LOG_DEBUG */
}
// setting the covariance
gravity.weight_of_the_measurement *= imu_measurements;
VECTOR_AS_VECT3(gravity.measured_direction, accelerometer_noise);
magneto.weight_of_the_measurement *= magnetometer_measurements;
VECTOR_AS_VECT3(magneto.measured_direction, magnetometer_noise);
add_set_of_three_measurements(&sigmaB, gravity, magneto);
#ifdef EKNAV_FROM_LOG_DEBUG
DISPLAY_FLOAT_RMAT(" B", attitude_profile_matrix);
DISPLAY_FLOAT_RMAT("sigmaB", sigmaB);
#endif /* EKNAV_FROM_LOG_DEBUG */
// setting the initial orientation
q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, &sigma_q);
orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body);
baro_0_height += pos_0_ecef.norm();
struct DoubleEulers sigma_eu = sigma_euler_from_sigma_q(q_ned2body, sigma_q);
orientation_cov_0 = EULER_AS_VECTOR3D(sigma_eu);
#if WITH_GPS
pos_cov_0 = 10*gps_pos_noise / gps_measurements;
speed_cov_0 = 10*gps_speed_noise / gps_measurements;
#endif // WITH_GPS
return e;
}
static void ahrs_do_update_mag(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
printf("Mag update.\n");
#endif
MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
/* P_prio = P */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_Pprio[i][j] = bafl_P[i][j];
}
}
/*
* set up measurement matrix
*
* h = [236.; -2.; 396.];
* hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
* Hm = Cnb * hx;
* H = [ 0 0 0 0 0
* 0 0 Cnb*hx 0 0 0
* 0 0 0 0 0 ];
* */
/*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
//rest is zero
/**********************************************
* compute Kalman gain K
*
* K = P_prio * H_T * inv(S)
* S = H * P_prio * HT + R
*
* K = P_prio * HT * inv(H * P_prio * HT + R)
*
**********************************************/
/* covariance residual S = H * P_prio * HT + R */
/* temp_S(3x6) = H(3x6) * P_prio(6x6)
*
* only third column of H is non-zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
}
}
/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
*
* only third row of HT is non-zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
}
bafl_S[i][i] += bafl_R_mag;
}
/* invert S
*/
FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
*
* only third row of HT is non-zero
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
/* not computing zero elements */
bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
}
}
/* K(6x3) = temp_K(6x3) * invS(3x3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
}
}
/**********************************************
* Update filter state.
*
* The a priori filter state is zero since the errors are corrected after each update.
*
* X = X_prio + K (y - H * X_prio)
* X = K * y
**********************************************/
/* innovation
* y = Cnb * [hx; hy; hz] - mag
*/
FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
FLOAT_VECT3_SUB(bafl_ym, bafl_mag);
/* X(6) = K(6x3) * y(3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
bafl_X[i] = bafl_K[i][0] * bafl_ym.x;
bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
}
/**********************************************
* Update the filter covariance.
*
* P = P_prio - K * H * P_prio
* P = ( I - K * H ) * P_prio
*
**********************************************/
/* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
*
* last 3 columns of H are zero
*/
for (i = 0; i < 6; i++) {
for (j = 0; j < 6; j++) {
if (i == j) {
bafl_tempP[i][j] = 1.;
} else {
bafl_tempP[i][j] = 0.;
}
if (j == 2) { /* omit the parts where H is zero */
for (k = 0; k < 3; k++) {
bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
}
}
}
}
/* P(6x6) = temp(6x6) * P_prio(6x6)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
for (k = 1; k < BAFL_SSIZE; k++) {
bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
}
}
}
#ifdef LKF_PRINT_P
printf("Pum=");
for (i = 0; i < 6; i++) {
printf("[");
for (j = 0; j < 6; j++) {
printf("%f\t", bafl_P[i][j]);
}
printf("]\n ");
}
printf("\n");
#endif
/**********************************************
* Correct errors.
*
*
**********************************************/
/* Error quaternion.
*/
QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
/* normalize */
float q_sq;
q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
printf("mag error quaternion q_sq > 1!!\n");
} else {
bafl_q_m_err.qi = sqrtf(1 - q_sq);
}
/* correct attitude
*/
FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
/* correct gyro bias
*/
RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
RATES_SUB(bafl_bias, bafl_b_m_err);
/*
* compute all representations
*/
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
AHRS_TO_BFP();
AHRS_LTP_TO_BODY();
}
void ahrs_update_mag(void) {
struct FloatVect3 imu_h;
MAGS_FLOAT_OF_BFP(imu_h, imu.mag);
update_state(&ahrs_impl.mag_h, &imu_h, &ahrs_impl.mag_noise);
reset_state();
}
