diff --git a/sw/in_progress/inertial/C/Makefile b/sw/in_progress/inertial/C/Makefile index 27fe20ca94..9d8fb28af4 100644 --- a/sw/in_progress/inertial/C/Makefile +++ b/sw/in_progress/inertial/C/Makefile @@ -55,6 +55,15 @@ tilt_fast_ekf: $(OBJS_TILT_FAST_EKF) # # +OBJS_AHRS_EULER_EKF = ahrs_euler_ekf.o \ + ahrs_utils.o \ + ahrs_data.o \ + ahrs_display.o \ + ekf.o \ + matrix.o + +ahrs_euler_ekf: $(OBJS_AHRS_EULER_EKF) + gcc -o $@ $^ $(LDFLAGS) diff --git a/sw/in_progress/inertial/C/ahrs_data.c b/sw/in_progress/inertial/C/ahrs_data.c index 1e5712fd07..f962bdb186 100644 --- a/sw/in_progress/inertial/C/ahrs_data.c +++ b/sw/in_progress/inertial/C/ahrs_data.c @@ -141,7 +141,25 @@ struct ahrs_data* ahrs_data_read_log(const char* filename) { return ad; } -void ahrs_data_save_state(struct ahrs_data* ad, int idx, double* X, double* P) { +void ahrs_data_save_state_euler(struct ahrs_data* ad, int idx, double* X, double* P) { + ad->est_phi[idx] = X[0]; + ad->est_theta[idx] = X[1]; + ad->est_psi[idx] = X[2]; + + ad->est_bias_p[idx] = X[3]; + ad->est_bias_q[idx] = X[4]; + ad->est_bias_r[idx] = X[5]; + + ad->P11[idx] = P[0]; + ad->P22[idx] = P[1*7 + 1]; + ad->P33[idx] = P[2*7 + 2]; + ad->P44[idx] = P[3*7 + 3]; + ad->P55[idx] = P[4*7 + 4]; + ad->P66[idx] = P[5*7 + 5]; + +} + +void ahrs_data_save_state_quat(struct ahrs_data* ad, int idx, double* X, double* P) { double eulers[3]; eulers_of_quat(eulers, X); ad->est_phi[idx] = eulers[0]; diff --git a/sw/in_progress/inertial/C/ahrs_data.h b/sw/in_progress/inertial/C/ahrs_data.h index 00ee0f8967..c6d22511fd 100644 --- a/sw/in_progress/inertial/C/ahrs_data.h +++ b/sw/in_progress/inertial/C/ahrs_data.h @@ -53,7 +53,8 @@ struct ahrs_data { extern struct ahrs_data* ahrs_data_new(int len, double dt); extern struct ahrs_data* ahrs_data_read_log(const char* filename); -extern void ahrs_data_save_state(struct ahrs_data* ad, int idx, double* X, double* P); +extern void ahrs_data_save_state_euler(struct ahrs_data* ad, int idx, double* X, double* P); +extern void ahrs_data_save_state_quat(struct ahrs_data* ad, int idx, double* X, double* P); extern void ahrs_data_save_measure(struct ahrs_data* ad, int idx); extern void ahrs_data_save_state(struct ahrs_data* ad, int idx, double* X, double* P); diff --git a/sw/in_progress/inertial/C/ahrs_euler_ekf.c b/sw/in_progress/inertial/C/ahrs_euler_ekf.c new file mode 100644 index 0000000000..b2cdc84bc7 --- /dev/null +++ b/sw/in_progress/inertial/C/ahrs_euler_ekf.c @@ -0,0 +1,192 @@ +#include "ahrs_data.h" +#include "ahrs_display.h" +#include "ahrs_utils.h" +#include "ekf.h" + +#include +#include + +static struct ahrs_data* ad; +#define UPDATE_PHI 0 +#define UPDATE_THETA 1 +#define UPDATE_PSI 2 +#define UPDATE_NB 3 +static int ahrs_state; + + +void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) { + *dt = ad->dt; + + double p = u[0] - X[3]; + double q = u[1] - X[4]; + double r = u[2] - X[5]; + + double phi = X[0]; + double theta = X[1]; + // double psi = X[2]; + + Xdot[0] = p + sin(phi)*tan(theta) * q + cos(phi)*tan(theta) * r; + Xdot[1] = cos(phi) * q - sin(phi) * r; + Xdot[2] = sin(phi)/cos(theta) * q + cos(phi)/cos(theta) * r; + Xdot[3] = 0.; + Xdot[4] = 0.; + Xdot[5] = 0.; + + F[0*6+0] = cos(phi)*tan(theta)*q - sin(phi)*tan(theta)*r; + F[0*6+1] = 1/(1+theta*theta) * (sin(phi)*q+cos(phi)*r); + F[0*6+2] = 0.; + F[0*6+3] = -1.; + F[0*6+4] = -sin(phi)*tan(theta); + F[0*6+5] = -cos(phi)*tan(theta); + + F[1*6+0] = -sin(phi)*q - cos(phi)*r; + F[1*6+1] = 0.; + F[1*6+2] = 0.; + F[1*6+3] = 0.; + F[1*6+4] = -cos(phi); + F[1*6+5] = sin(phi); + + F[2*6+0] = cos(phi)/cos(theta)*q - sin(phi)/cos(theta)*r; + F[2*6+1] = sin(theta)/(cos(theta)*cos(theta))*(sin(phi)*q+cos(phi)*r); + F[2*6+2] = 0.; + F[2*6+3] = 0.; + F[2*6+4] = -sin(phi)/cos(theta); + F[2*6+5] = -cos(phi)/cos(theta); + + F[3*6+0] = 0.; + F[3*6+1] = 0.; + F[3*6+2] = 0.; + F[3*6+3] = 0.; + F[3*6+4] = 0.; + F[3*6+5] = 0.; + + F[4*6+0] = 0.; + F[4*6+1] = 0.; + F[4*6+2] = 0.; + F[4*6+3] = 0.; + F[4*6+4] = 0.; + F[4*6+5] = 0.; + + F[5*6+0] = 0.; + F[5*6+1] = 0.; + F[5*6+2] = 0.; + F[5*6+3] = 0.; + F[5*6+4] = 0.; + F[5*6+5] = 0.; + +} + +void linear_measure(double *y, double* err, double*X, double *H) { + *err = y[0] - X[ahrs_state]; + switch (ahrs_state) { + case UPDATE_PHI: { + WARP(*err, M_PI); + H[0] = 1.; + H[1] = 0.; + H[2] = 0.; + break; + } + case UPDATE_THETA: { + WARP(*err, M_PI_2); + H[0] = 0.; + H[1] = 1.; + H[2] = 0.; + break; + } + case UPDATE_PSI: { + WARP(*err, M_PI); + H[0] = 0.; + H[1] = 0.; + H[2] = 1.; + break; + } + } + H[3] = 0.; + H[4] = 0.; + H[5] = 0.; +} + + +#define P0E 1.0 +#define P0B 1.0 + +#define QE 0.001 +#define QB 0.008 + +void run_ekf (void) { + /* initial state covariance matrix */ + double P[6*6] = {P0E, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, P0E, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, P0E, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, P0B, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, P0B, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, P0B }; + /* model noise covariance matrix */ + double Q[6*6] = { QE, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, QE, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, QE, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, QB, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, QB, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, QB }; + /* measurement noise covariance matrix */ + double R[1]={1.7}; + /* state [phi, theta, psi, bp, bq, br] */ + double X[6]; + /* measure */ + double Y[1]; + /* command */ + double U[3] = {0.0, 0.0, 0.0}; + + struct ekf_filter* ekf; + ekf = ekf_filter_new(6, 1, Q, R, linear_filter, linear_measure); + ahrs_euler_init(ad, 150, X); + ekf_filter_reset(ekf, X, P); + int iter; + + for (iter=0; iter < ad->nb_samples; iter++) { + U[0] = ad->gyro_p[iter]; + U[1] = ad->gyro_q[iter]; + U[2] = ad->gyro_r[iter]; + ekf_filter_predict(ekf, U); + + ahrs_state = iter%3; + switch (ahrs_state) { + case UPDATE_PHI: + Y[0] = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]); + break; + case UPDATE_THETA: + Y[0] = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]); + break; + case UPDATE_PSI: { + double m_phi = phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]); + double m_theta = theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]); + Y[0] = psi_of_mag(m_phi, m_theta, ad->mag_x[iter], ad->mag_y[iter], ad->mag_z[iter]); + break; + } + } + ekf_filter_update(ekf, Y); + + ekf_filter_get_state(ekf, X, P); + ahrs_data_save_state_euler(ad, iter, X, P); + ahrs_data_save_measure(ad, iter); + + } + + +} + + + +int +main(int argc, char *argv[]) { + gtk_init(&argc, &argv); + + //ad = ahrs_data_read_log("../data/log_ahrs_bug"); + ad = ahrs_data_read_log("../data/log_ahrs_roll"); + //ad = ahrs_data_read_log("../data/log_ahrs_yaw_pitched"); + run_ekf(); + ahrs_display(ad); + gtk_main(); + + return 0; +} diff --git a/sw/in_progress/inertial/C/ahrs_quat_ekf.c b/sw/in_progress/inertial/C/ahrs_quat_ekf.c index 2c6ac556b2..90aa1ae7f7 100644 --- a/sw/in_progress/inertial/C/ahrs_quat_ekf.c +++ b/sw/in_progress/inertial/C/ahrs_quat_ekf.c @@ -108,6 +108,9 @@ void linear_measure(double *y, double* err, double*X, double *H) { break; } } + H[4] = 0.; + H[5] = 0.; + H[6] = 0.; } #define P0Q 1.0 @@ -144,7 +147,7 @@ void run_ekf (void) { struct ekf_filter* ekf; ekf = ekf_filter_new(7, 1, Q, R, linear_filter, linear_measure); - ahrs_init(ad, 150, X); + ahrs_quat_init(ad, 150, X); ekf_filter_reset(ekf, X, P); /* filter run */ @@ -174,7 +177,7 @@ void run_ekf (void) { norm_quat(X); // printf("P66 %f\n", P[6*7 + 6]); ekf_filter_get_state(ekf, X, P); - ahrs_data_save_state(ad, iter, X, P); + ahrs_data_save_state_quat(ad, iter, X, P); ahrs_data_save_measure(ad, iter); } } diff --git a/sw/in_progress/inertial/C/ahrs_utils.c b/sw/in_progress/inertial/C/ahrs_utils.c index 708d248594..ec176767ac 100644 --- a/sw/in_progress/inertial/C/ahrs_utils.c +++ b/sw/in_progress/inertial/C/ahrs_utils.c @@ -70,15 +70,15 @@ void norm_quat(double* quat) { quat[3] /= mag; } -void ahrs_init(struct ahrs_data* ad, int len, double* X) { +void ahrs_euler_init(struct ahrs_data* ad, int len, double* X) { double init_phi = 0.; double init_theta = 0.; double init_mx = 0.; double init_my = 0.; double init_mz = 0.; + X[3] = 0.; X[4] = 0.; X[5] = 0.; - X[6] = 0.; int iter; for (iter = 0; iter < len; iter++) { /* average attitude */ @@ -88,9 +88,9 @@ void ahrs_init(struct ahrs_data* ad, int len, double* X) { init_my += ad->mag_y[iter]; init_mz += ad->mag_z[iter]; /* sum gyros */ - X[4] += ad->gyro_p[iter]; - X[5] += ad->gyro_q[iter]; - X[6] += ad->gyro_r[iter]; + X[3] += ad->gyro_p[iter]; + X[4] += ad->gyro_q[iter]; + X[5] += ad->gyro_r[iter]; } init_phi /= (double)len; init_theta /= (double)len; @@ -98,10 +98,21 @@ void ahrs_init(struct ahrs_data* ad, int len, double* X) { init_my /= (double)len; init_mz /= (double)len; double init_psi = psi_of_mag(init_phi, init_theta, init_mx, init_my, init_mz); - quat_of_eulers(X, init_phi, init_theta, init_psi); + X[0] = init_phi; + X[1] = init_theta; + X[2] = init_psi; + X[3] /= (double)len; X[4] /= (double)len; X[5] /= (double)len; - X[6] /= (double)len; - printf("ahrs init : eulers [%f %f %f] biases [%f %f %f]\n", init_phi, init_theta, init_psi, X[4], X[5], X[6]); + printf("ahrs init : eulers [%f %f %f] biases [%f %f %f]\n", init_phi, init_theta, init_psi, X[3], X[4], X[5]); +} + +void ahrs_quat_init(struct ahrs_data* ad, int len, double* X) { + ahrs_euler_init(ad, len, X); + + X[6] = X[5]; + X[5] = X[4]; + X[4] = X[3]; + quat_of_eulers(X, X[0], X[1], X[2]); } diff --git a/sw/in_progress/inertial/C/ahrs_utils.h b/sw/in_progress/inertial/C/ahrs_utils.h index 232453d4eb..c4e779bb87 100644 --- a/sw/in_progress/inertial/C/ahrs_utils.h +++ b/sw/in_progress/inertial/C/ahrs_utils.h @@ -10,7 +10,8 @@ extern void quat_of_eulers(double* quat, double phi, double theta, double psi ); extern void eulers_of_quat(double* euler, double* quat); extern void norm_quat(double* quat); -extern void ahrs_init(struct ahrs_data* ad, int len, double* X); +extern void ahrs_quat_init(struct ahrs_data* ad, int len, double* X); +extern void ahrs_euler_init(struct ahrs_data* ad, int len, double* X); #define WARP(x, y) { \ while ( x < -y ) \