Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 01:53:48 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-07-31 11:43:20 +00:00
parent d9f515f763
commit ef0af38f71
7 changed files with 248 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/inertial/C/Makefile
+9
View File
@@ -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)
sw/in_progress/inertial/C/ahrs_data.c
+19 -1
View File
@@ -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];
sw/in_progress/inertial/C/ahrs_data.h
+2 -1
View File
@@ -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);
sw/in_progress/inertial/C/ahrs_euler_ekf.c
+192
View File
@@ -0,0 +1,192 @@
#include "ahrs_data.h"
#include "ahrs_display.h"
#include "ahrs_utils.h"
#include "ekf.h"
#include <string.h>
#include <math.h>
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;
}
sw/in_progress/inertial/C/ahrs_quat_ekf.c
+5 -2
View File
@@ -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);
}
}
sw/in_progress/inertial/C/ahrs_utils.c
+19 -8
View File
@@ -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]);
}
sw/in_progress/inertial/C/ahrs_utils.h
+2 -1
View File
@@ -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 ) \