[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6273] everything seems to work for the Kalman filte
|
From: |
Martin Dieblich |
|
Subject: |
[paparazzi-commits] [6273] everything seems to work for the Kalman filter |
|
Date: |
Tue, 26 Oct 2010 17:09:15 +0000 |
Revision: 6273
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6273
Author: mdieblich
Date: 2010-10-26 17:09:15 +0000 (Tue, 26 Oct 2010)
Log Message:
-----------
everything seems to work for the Kalman filter
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile
paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c
paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log.h
paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log_to_ascii.c
paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile 2010-10-26 16:26:41 UTC
(rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/Makefile 2010-10-26 17:09:15 UTC
(rev 6273)
@@ -1,6 +1,6 @@
raw_log_to_ascii: raw_log_to_ascii.c
- gcc -I../../ -std=gnu99 -Wall raw_log_to_ascii.c -o raw_log_to_ascii
+ gcc -I../../ -I../../../include -std=gnu99 -Wall raw_log_to_ascii.c
-DOVERO_LINK_MSG_UP=AutopilotMessageVIUp
-DOVERO_LINK_MSG_DOWN=AutopilotMessageVIDown -o raw_log_to_ascii
fetch_log:
@@ -17,10 +17,13 @@
ins_qkf_observe_gps_pvt.cpp \
ins_qkf_observe_gps_p.cpp
+eknavOnLogFlags = -DOVERO_LINK_MSG_UP=AutopilotMessageVIUp \
+ -DOVERO_LINK_MSG_DOWN=AutopilotMessageVIDown \
+ -DEKNAV_FROM_LOG_DEBUG
run_filter_on_log: ./libeknav_from_log.cpp $(LIBEKNAV_SRCS)
../../math/pprz_geodetic_double.c ../../math/pprz_geodetic_float.c
- g++ -I/usr/include/eigen2 -I../.. -I../../../include
-I../../../../var/FY -DOVERO_LINK_MSG_UP=AutopilotMessageVIUp
-DOVERO_LINK_MSG_DOWN=AutopilotMessageVIDown -DEKNAV_FROM_LOG_DEBUG -o $@ $^
+ g++ -I/usr/include/eigen2 -I../.. -I../../../include
-I../../../../var/FY $(eknavOnLogFlags) -o $@ $^
clean:
-rm -f *.o *~ *.d
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/estimate_attitude.c
2010-10-26 17:09:15 UTC (rev 6273)
@@ -66,6 +66,9 @@
}
}
+ #ifdef EKNAV_FROM_LOG_DEBUG
+ printf("Number of iterations: %4i\n", k);
+ #endif
if(k==maximum_iterations){
printf("Orientation did not converge. Using maximum uncertainty\n");
//FLOAT_QUAT_ZERO(x_k);
@@ -77,7 +80,7 @@
/* This function generates the "K"-matrix out of an attitude profile matrix
*
* I don't know the real name of the "K"-Matrix, but everybody (see References
from the other functions)
- * names it "K", so I do it as well.
+ * calls it "K", so I do it as well.
*/
struct DoubleMat44 generate_K_matrix(struct DoubleMat33 B){
struct DoubleMat44 K;
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_p.cpp
2010-10-26 17:09:15 UTC (rev 6273)
@@ -65,9 +65,22 @@
#if BARO_CENTER_OF_MASS
void
basic_ins_qkf::obs_baro_report(double altitude, double baro_error){
+ double height_state = avg_state.position.norm();
+ Matrix<double, 1, 3> H = avg_state.position.transpose()/height_state;
+ Matrix<double, 1, 1> innovation_cov = H * cov.block<3,3>(6,6) *
H.transpose();
+ Matrix<double, 12, 1> kalman_gain = cov.block<12, 3>(0,6) * H.transpose() /
(innovation_cov(0)+baro_error);
+
+ Matrix<double, 12, 1> update = kalman_gain * (altitude - height_state);
+ //std::cout <<"Delta: " << (altitude - height_state) << std::endl;
+ //std::cout <<"Update: " << update.block<6,1>(0,0).transpose() << "\t" <<
update.block<6,1>(6,0).transpose() << std::endl;
+ Quaterniond rotor = avg_state.apply_kalman_vec_update(update);
+ counter_rotate_cov(rotor);
+
+ cov.part<Eigen::SelfAdjoint>() -= kalman_gain * H * cov.block<3, 12>(6, 0);
+
}
-#else
+#else /* BARO_CENTER_OF_MASS */
void
basic_ins_qkf::obs_baro_report(double altitude, double baro_error,
Matrix<double, 3, 3> ecef2enu, const Vector3d& pos_0){
Matrix<double, 1, 3> h = ecef2enu.block<1, 3>(2,0);
@@ -83,5 +96,4 @@
Quaterniond rotor = avg_state.apply_kalman_vec_update(update);
counter_rotate_cov(rotor);
}
-#endif
-
+#endif /* BARO_CENTER_OF_MASS */
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
2010-10-26 17:09:15 UTC (rev 6273)
@@ -5,6 +5,8 @@
struct LtpDef_d current_ltp;
// NOTE-END
+unsigned int entry_counter;
+
FILE* ins_logfile; // note: initilaized in init_ins_state
//useless initialization (I hate C++)
@@ -15,7 +17,8 @@
USING_PART_OF_NAMESPACE_EIGEN
int main(int, char *argv[]) {
-
+
+ entry_counter = 0;
printf("==============================\nRunning libeknav from
File...\n==============================\n");
int raw_log_fd = open(argv[1], O_RDONLY);
@@ -28,6 +31,7 @@
printf("Initialisation...\n");
struct raw_log_entry e = first_entry_after_initialisation(raw_log_fd);
printf("Starting at t = %5.2f s\n", e.time);
+ printf("entry counter: %i\n", entry_counter);
main_init();
printf("Running filter from file...\n");
main_run_from_file(raw_log_fd, e);
@@ -58,6 +62,7 @@
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"
@@ -70,14 +75,19 @@
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 = 1./(double)(imu_frequency); //
originally 1/(imu_frequency*gravity.norm()
+ 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 = 1./(double)(mag_frequency); //
originally 1/(mag_frequency*reference_direction.norm()
-
+ 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);
@@ -92,44 +102,69 @@
#ifdef EKNAV_FROM_LOG_DEBUG
int imu_ready = 0,
mag_ready = 0,
+ baro_ready = 0,
gps_ready = 0;
#endif
- for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements,
magnetometer_measurements, gps_measurements); e =
read_raw_log_entry(file_descriptor, &read_ok)){
- if(IMU_READY(e.data_valid)){
+ 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_AS_VECTOR3D(e.gyro), imu_measurements);
+ bias_0 = NEW_MEAN(bias_0, RATES_BFP_AS_VECTOR3D(e.message.gyro),
imu_measurements);
// update the attitude profile matrix
- VECT3_COPY(gravity.measured_direction,e.accel);
+ ACCELS_FLOAT_OF_BFP(gravity.measured_direction,e.message.accel);
add_orientation_measurement(&attitude_profile_matrix, gravity);
}
- if(MAG_READY(e.data_valid)){
+ if(MAG_READY(e.message.valid_sensors)){
magnetometer_measurements++;
// update the attitude profile matrix
- VECT3_COPY(magneto.measured_direction,e.mag);
+ 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(GPS_READY(e.data_valid)){
+ if(BARO_READY(e.message.valid_sensors)){
+ baro_measurements++;
+ printf("baro_0_height before: %7f\n", baro_0_height);
+ //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.ecef_pos)/100,
gps_measurements);
- speed_0_ecef = NEW_MEAN(speed_0_ecef, VECT3_AS_VECTOR3D(e.ecef_vel)/100,
gps_measurements);
+ 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){ imu_ready =
!NOT_ENOUGH_IMU_MEASUREMENTS(imu_measurements); }
- else if(imu_ready==1){printf("IMU READY %3i %3i %3i\n", imu_measurements,
magnetometer_measurements, gps_measurements); imu_ready = 2;}
- if(mag_ready==0){ mag_ready =
!NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(magnetometer_measurements); }
- else if(mag_ready==1){printf("MAG READY %3i %3i %3i\n", imu_measurements,
magnetometer_measurements, gps_measurements); mag_ready = 2;}
- if(gps_ready==0){ gps_ready =
!NOT_ENOUGH_GPS_MEASUREMENTS(gps_measurements);}
- else if(gps_ready==1){printf("GPS READY %3i %3i %3i\n", imu_measurements,
magnetometer_measurements, gps_measurements); gps_ready = 2;}
+ 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
}
// setting the covariance
@@ -144,9 +179,14 @@
DISPLAY_FLOAT_RMAT("sigmaB", sigmaB);
#endif
+ // 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);
+ printf("baro_0_height before: %7f\n", baro_0_height);
+ baro_0_height += pos_0_ecef.norm();
+ printf("baro_0_height after: %7f\n", baro_0_height);
+
struct DoubleEulers sigma_eu = sigma_euler_from_sigma_q(q_ned2body, sigma_q);
orientation_cov_0 = EULER_AS_VECTOR3D(sigma_eu);
#if WITH_GPS
@@ -164,13 +204,13 @@
int t = 10*(int)(1+e.time*0.1);
while (read_ok) {
if(e.time>t){
- printf("%6.2f s\n", e.time);
+ printf("%6.2fs %6i\n", e.time, entry_counter);
t += 10;
}
- if ((e.time<22.58)||(e.time>22.6)){
- main_run_ins(e.data_valid);
+ //if ((e.time<22.58)||(e.time>22.6)){
+ main_run_ins(e.message.valid_sensors);
print_estimator_state(e.time);
- }
+ //}
e = read_raw_log_entry(file_descriptor, &read_ok);
}
}
@@ -191,15 +231,17 @@
}
#endif
+ if(BARO_READY(data_valid)){
+ ins.obs_baro_report(baro_0_height+imu_baro_height, baro_noise);
+ }
+
if(GPS_READY(data_valid)){
ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100,
VECT3_AS_VECTOR3D(imu_ecef_vel)/100, 10*gps_pos_noise, 10*gps_speed_noise);
Matrix<double, 3, 3> ecef2enu;
RMAT_TO_EIGENMATRIX(ecef2enu,current_ltp.ltp_of_ecef.m);
- ins.obs_baro_report(0, 1, ecef2enu, pos_0_ecef);
}
-
}
@@ -230,13 +272,13 @@
printf("Orientation % 7.2f %6.1f° +-%6.1f°\n", orientation_0.y(),
eu_ecef2body.theta*180*M_1_PI, orientation_cov_0(1)*180*M_1_PI);
printf("Orientation % 7.2f %6.1f° +-%6.1f°\n", orientation_0.z(),
eu_ecef2body.psi*180*M_1_PI, orientation_cov_0(2)*180*M_1_PI);
printf("\n");
- printf("Position % 9.0f +-%7.2f\n", pos_0_ecef(0), pos_cov_0(0));
- printf("Position % 9.0f +-%7.2f\n", pos_0_ecef(1), pos_cov_0(1));
- printf("Position % 9.0f +-%7.2f\n", pos_0_ecef(2), pos_cov_0(2));
+ printf("Position % 9.0f m +-%7.2f\n", pos_0_ecef(0), pos_cov_0(0));
+ printf("Position % 9.0f m +-%7.2f\n", pos_0_ecef(1), pos_cov_0(1));
+ printf("Position % 9.0f m +-%7.2f\n", pos_0_ecef(2), pos_cov_0(2));
printf("\n");
- printf("Velocity % 7.2f +-%7.2f\n", speed_0_ecef(0),
speed_cov_0(0));
- printf("Velocity % 7.2f +-%7.2f\n", speed_0_ecef(1),
speed_cov_0(1));
- printf("Velocity % 7.2f +-%7.2f\n", speed_0_ecef(2),
speed_cov_0(2));
+ printf("Velocity % 7.2f m/s +-%7.2f\n", speed_0_ecef(0),
speed_cov_0(0));
+ printf("Velocity % 7.2f m/s +-%7.2f\n", speed_0_ecef(1),
speed_cov_0(1));
+ printf("Velocity % 7.2f m/s +-%7.2f\n", speed_0_ecef(2),
speed_cov_0(2));
printf("\n");
Matrix<double, 12, 1> diag_cov;
@@ -285,6 +327,14 @@
FLOAT_QUAT_COMP_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body);
+ #ifdef EKNAV_FROM_LOG_DEBUG
+ printf("Right after initialization:\n");
+ DISPLAY_FLOAT_QUAT("\t ned2body quaternion:", q_ned2body);
+ DISPLAY_FLOAT_QUAT("\tecef2enu quaternion:", q_ecef2enu);
+ DISPLAY_FLOAT_QUAT("\tecef2ned quaternion:", q_ecef2ned);
+ DISPLAY_FLOAT_QUAT("\tecef2body quaternion:", q_ecef2body);
+ #endif
+
return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body);
}
@@ -319,17 +369,19 @@
struct raw_log_entry e;
ssize_t nb_read = read(file_descriptor, &e, sizeof(e));
*read_ok = (nb_read == sizeof(e));
+ entry_counter ++;
- COPY_RATES_ACCEL_TO_IMU_FLOAT(e);
- COPY_MAG_TO_IMU_FLOAT(e);
- COPY_GPS_TO_IMU(e);
+ COPY_BARO_TO_IMU(e.message);
+ COPY_RATES_ACCEL_TO_IMU_FLOAT(e.message);
+ COPY_MAG_TO_IMU_FLOAT(e.message);
+ COPY_GPS_TO_IMU(e.message);
return e;
}
static struct raw_log_entry next_GPS(int file_descriptor){
uint8_t read_ok;
struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
- while ((read_ok)&&(!GPS_READY(e.data_valid))) {
+ while ((read_ok)&&(!GPS_READY(e.message.valid_sensors))) {
e = read_raw_log_entry(file_descriptor, &read_ok);
}
return e;
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
2010-10-26 17:09:15 UTC (rev 6273)
@@ -33,6 +33,7 @@
#include "fms/libeknav/raw_log.h"
/* our sensors */
struct ImuFloat imu_float;
+ double imu_baro_height;
struct EcefCoor_i imu_ecef_pos,
imu_ecef_vel;
@@ -42,26 +43,15 @@
/* constants */
/** Compilation-control **/
#define UPDATE_WITH_GRAVITY 1
-#define SYNTHETIC_MAG_MODE 0
#define FILTER_OUTPUT_IN_NED 1
-#define WITH_GPS 0
+#define WITH_GPS 1
#define PRINT_MAG 0
#define PRINT_GPS 0
#define PRINT_EULER_NED 0
/** baro-sensor **/
-// I only want the function
-#define NPS_SENSORS_PARAMS <math.h>
-// Params taken from trunk/conf/simulator/nps/nps_sensors_params_booz2_a1.h
-#define NPS_BARO_QNH 900.
-#define NPS_BARO_SENSITIVITY 17.066667
-#define NPS_BARO_DT (1./100.)
-#define NPS_BARO_NOISE_STD_DEV 5.e-2
-// include stuff
-#include "../simulator/nps/nps_sensor_baro.h"
-#include "../simulator/nps/nps_sensor_baro.c"
/** geodetic **/
@@ -80,6 +70,7 @@
/** initial state **/
struct LlaCoor_f pos_0_lla;
+double baro_0_height = 0;
Vector3d pos_0_ecef = Vector3d::Zero();
Vector3d speed_0_ecef = Vector3d::Zero();
Vector3d bias_0 = Vector3d::Zero();
@@ -113,6 +104,9 @@
const unsigned short gps_frequency = 4;
//const Vector3d gps_pos_noise = Vector3d::Ones() *10 *10 ;
+const double baro_noise = 0.25;
+#define BARO_SCALING 10.0
+
//const double mag_error = 2.536e-3;
//const Vector3d gyro_white_noise ( 1.1328*1.1328e-4,
0.9192*0.9192e-4, 1.2291*1.2291e-4);
//const Vector3d gyro_stability_noise ( -1.7605*1.7605e-4,
0.5592*0.5592e-4, 1.1486*1.1486e-4);
@@ -129,31 +123,39 @@
static struct raw_log_entry next_GPS(int);
static void print_estimator_state(double);
+#define AC_ID 210
#define INS_LOG_FILE "log_ins_test3.data"
/* Other */
/** Average-Calculation **/
-#define MINIMAL_IMU_MEASUREMENTS 1000
-#define MINIMAL_MAGNETIC_FIELD_MEASUREMENTS 30
-#define MINIMAL_GPS_MEASUREMENTS 10
+#define MINIMAL_IMU_MEASUREMENTS 1000
+#define MINIMAL_MAGNETIC_FIELD_MEASUREMENTS 30
+#define MINIMAL_BARO_MEASUREMENTS 30
+#define MINIMAL_GPS_MEASUREMENTS 10
-#define NOT_ENOUGH_MEASUREMENTS(imu, mag, gps)
(NOT_ENOUGH_IMU_MEASUREMENTS(imu) || \
+#define NOT_ENOUGH_MEASUREMENTS(imu,mag,baro,gps)
(NOT_ENOUGH_IMU_MEASUREMENTS(imu) || \
NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(mag) || \
+
NOT_ENOUGH_BARO_MEASUREMENTS(baro) || \
NOT_ENOUGH_GPS_MEASUREMENTS(gps) )
-#define NOT_ENOUGH_IMU_MEASUREMENTS(imu)
((imu)<(MINIMAL_IMU_MEASUREMENTS) )
-#define NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(mag)
((mag)<(MINIMAL_MAGNETIC_FIELD_MEASUREMENTS))
-#define NOT_ENOUGH_GPS_MEASUREMENTS(gps)
((gps)<(MINIMAL_GPS_MEASUREMENTS) )
+#define NOT_ENOUGH_IMU_MEASUREMENTS(imu) ((imu)
<(MINIMAL_IMU_MEASUREMENTS) )
+#define NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(mag) ((mag)
<(MINIMAL_MAGNETIC_FIELD_MEASUREMENTS))
+#define NOT_ENOUGH_BARO_MEASUREMENTS(baro)
((baro)<(MINIMAL_BARO_MEASUREMENTS) )
+#define NOT_ENOUGH_GPS_MEASUREMENTS(gps) ((gps)
<(MINIMAL_GPS_MEASUREMENTS) )
#define NEW_MEAN(old_mean, new_observation, index)
(((old_mean)*(index-1)+(new_observation))/(index))
/** Sensors **/
+#define INT32_BARO_FRAC 8
+#define BARO_FLOAT_OF_BFP(_ai) (FLOAT_OF_BFP((_ai),
INT32_BARO_FRAC)*BARO_SCALING)
+
#define COPY_RATES_ACCEL_TO_IMU_FLOAT(pointer){
\
RATES_FLOAT_OF_BFP(imu_float.gyro, pointer.gyro); \
ACCELS_FLOAT_OF_BFP(imu_float.accel, pointer.accel); \
}
#define COPY_MAG_TO_IMU_FLOAT(pointer) MAGS_FLOAT_OF_BFP(imu_float.mag,
pointer.mag)
+#define COPY_BARO_TO_IMU(pointer) imu_baro_height =
-BARO_FLOAT_OF_BFP(pointer.pressure_absolute)
#define COPY_GPS_TO_IMU(pointer){
\
VECT3_COPY(imu_ecef_pos, pointer.ecef_pos);
\
VECT3_COPY(imu_ecef_vel, pointer.ecef_vel);
\
@@ -161,8 +163,9 @@
#define IMU_READY(data_valid) (data_valid & (1<<VI_IMU_DATA_VALID))
+#define MAG_READY(data_valid) (data_valid & (1<<VI_MAG_DATA_VALID))
+#define BARO_READY(data_valid) (data_valid & (1<<VI_BARO_ABS_DATA_VALID))
#define GPS_READY(data_valid) (data_valid & (1<<VI_GPS_DATA_VALID))
-#define MAG_READY(data_valid) (data_valid & (1<<VI_MAG_DATA_VALID))
#define CLOSE_TO_GRAVITY(accel)
(ABS(FLOAT_VECT3_NORM(accel)-GRAVITY)<MAX_DISTANCE_FROM_GRAVITY_FOR_UPDATE)
@@ -190,3 +193,7 @@
mat.m[0], mat.m[1], mat.m[2], mat.m[3], mat.m[4], mat.m[5], \
mat.m[6], mat.m[7], mat.m[8]); \
}
+#define DISPLAY_FLOAT_QUAT(text, quat) { \
+ double quat_norm = NORM_VECT4(quat); \
+ printf("%s %f %f %f %f (%f)\n",text, quat.qi, quat.qx, quat.qy, quat.qz,
quat_norm); \
+ }
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
2010-10-26 17:09:15 UTC (rev 6273)
@@ -21,11 +21,12 @@
* # uses in comparison to Paparazzi.
#
* # See the documentation in doc/pprz_algebra for more
explanation. #
* #
#
- * # As a result...
#
- * # ... the euler angles are negated
#
- * # ... rotational matrices are transposed/inverted
#
- * # ... quaternions are composed.
#
+ * # As a result the euler angles are negated
#
* #
#
+ * # You should also pay attention to the multtiplications:
#
+ * # Although quaternions and matrices "from?to?" are unique
#
+ * # The order in the mutliplication might change
#
+ * #
#
* # Thank you for you attention :-)
#
* #
#
*
===================================================================
@@ -123,13 +124,9 @@
#define QUAT_ENU_FROM_TO_NED(from, to){ \
to.qi = from.qx + from.qy;
\
- DISPLAY_FLOAT_QUAT("\tstep_1", to); \
to.qx = -from.qi - from.qz; \
- DISPLAY_FLOAT_QUAT("\tstep_2", to); \
to.qy = -from.qi + from.qz; \
- DISPLAY_FLOAT_QUAT("\tstep_3", to); \
to.qz = from.qx - from.qy; \
- DISPLAY_FLOAT_QUAT("\tstep_4", to); \
QUAT_SMUL(to, to, M_SQRT1_2); \
}
@@ -150,7 +147,7 @@
#define EULER_AS_VECTOR3D(euler) -Vector3d(euler.phi, euler.theta, euler.psi);
#define RATES_AS_VECTOR3D(rates) Vector3d(rates.p,rates.q,rates.r)
#define RATES_BFP_AS_VECTOR3D(rates)
Vector3d(RATE_FLOAT_OF_BFP(rates.p),RATE_FLOAT_OF_BFP(rates.q),RATE_FLOAT_OF_BFP(rates.r))
-#define DOUBLEQUAT_AS_QUATERNIOND(quat) Quaterniond(quat.qi, -quat.qx,
-quat.qy, -quat.qz)
+#define DOUBLEQUAT_AS_QUATERNIOND(quat) Quaterniond(quat.qi, quat.qx, quat.qy,
quat.qz)
#define RMAT_TO_EIGENMATRIX(Eigen,c) Eigen <<
(c)[0],(c)[1],(c)[2],(c)[3],(c)[4],(c)[5],(c)[6],(c)[7],(c)[8]
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log.h 2010-10-26 16:26:41 UTC
(rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log.h 2010-10-26 17:09:15 UTC
(rev 6273)
@@ -1,8 +1,10 @@
#ifndef LIBEKNAV_RAW_LOG_H
#define LIBEKNAV_RAW_LOG_H
-#include "math/pprz_algebra_float.h"
+//#include "math/pprz_algebra_float.h"
+#include "fms/fms_autopilot_msg.h"
+/*
struct __attribute__ ((packed)) raw_log_entry{
float time;
struct FloatRates gyro;
@@ -13,6 +15,11 @@
int16_t pressure_absolute;
uint8_t data_valid;
};
+*/
+struct __attribute__ ((packed)) raw_log_entry{
+ float time;
+ struct AutopilotMessageVIUp message;
+};
#endif /* LIBEKNAV_RAW_LOG_H */
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log_to_ascii.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log_to_ascii.c
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/raw_log_to_ascii.c
2010-10-26 17:09:15 UTC (rev 6273)
@@ -9,16 +9,23 @@
-
+#include "math/pprz_algebra_double.h"
+#include "fms/fms_autopilot_msg.h"
#include "fms/libeknav/raw_log.h"
void print_raw_log_entry(struct raw_log_entry*);
-void build_fake_log(void);
+//void build_fake_log(void);
#define PRT(a) printf("%f ", a);
#define VI_GPS_DATA_VALID 2
#define GPS_READY(data_valid) (data_valid & (1<<VI_GPS_DATA_VALID))
+/*
+struct raw_log_entry __attribute__ ((packed)){
+ double time;
+ struct AutopilotMessageVIUp message;
+}
+*/
int main(int argc, char** argv) {
@@ -31,12 +38,14 @@
perror("opening log\n");
return -1;
}
+ //printf("%i\n", sizeof(struct raw_log_entry));
+ //return 0;
while (1) {
- struct raw_log_entry e;
- ssize_t nb_read = read(raw_log_fd, &e, sizeof(e));
- if (nb_read != sizeof(e)) break;
- print_raw_log_entry(&e);
+ struct raw_log_entry entry;
+ ssize_t nb_read = read(raw_log_fd, &entry, sizeof(entry));
+ if (nb_read != sizeof(entry)) break;
+ print_raw_log_entry(&entry);
printf("\n");
//printf("%f %f %f %f", e.time, e.gyro.p, e.gyro.q, e.gyro.r);
}
@@ -46,19 +55,24 @@
-void print_raw_log_entry(struct raw_log_entry* entry){
- printf("%f\t", entry->time);
- printf("%d\t", entry->data_valid);
- printf("%+f %+f %+f\t", entry->gyro.p, entry->gyro.q, entry->gyro.r);
- printf("%+f %+f %+f\t", entry->accel.x, entry->accel.y, entry->accel.z);
- printf("%+f %+f %+f\t", entry->mag.x, entry->mag.y, entry->mag.z);
- printf("%+f %+f %+f\t", entry->ecef_pos.x, entry->ecef_pos.y,
entry->ecef_pos.z);
- printf("%+f %+f %+f\t", entry->ecef_vel.x, entry->ecef_vel.y,
entry->ecef_vel.z);
- printf("%i", entry->pressure_absolute);
+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 build_fake_log(void) {
int raw_log_fd = open( "log_test3.bin", O_WRONLY|O_CREAT, 00644);
for (int i=0; i<5000; i++) {
@@ -68,3 +82,4 @@
}
close(raw_log_fd);
}
+*/
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.cpp
2010-10-26 17:09:15 UTC (rev 6273)
@@ -96,16 +96,18 @@
}
}
- uint8_t data_valid = main_dialog_with_io_proc();
+ //uint8_t data_valid = main_dialog_with_io_proc();
+ main_dialog_with_io_proc();
#if RUN_FILTER
main_run_ins(data_valid);
#endif
- main_rawlog_dump(data_valid);
+ //main_rawlog_dump(data_valid);
}
-static uint8_t main_dialog_with_io_proc() {
+//static uint8_t main_dialog_with_io_proc() {
+static void main_dialog_with_io_proc() {
DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(message);
uint8_t crc_valid;
@@ -113,9 +115,12 @@
// for (uint8_t i=0; i<6; i++)
msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
spi_link_send(&message_out, sizeof(struct AutopilotMessageCRCFrame),
&message_in, &crc_valid);
+ main_rawlog_dump(&message_in.payload.msg_up);
- struct AutopilotMessageVIUp *in = &message_in.payload.msg_up;
- //printf(" <%2i> ", in->valid_sensors);
+
+ if(GPS_READY(message_in.payload.msg_up.valid_sensors)){printf("GPS!\n");}
+ /*struct AutopilotMessageVIUp *in = &message_in.payload.msg_up;
+
if(IMU_READY(in->valid_sensors)){
COPY_RATES_ACCEL_TO_IMU_FLOAT(in);
}
@@ -128,7 +133,6 @@
}
if(BARO_READY(in->valid_sensors)){
- //printf(" BARO!!! ");
baro_measurement = in->pressure_absolute;
}
@@ -139,7 +143,7 @@
#endif
}
- return in->valid_sensors;
+ return in->valid_sensors;*/
}
@@ -280,19 +284,26 @@
static void main_rawlog_init(const char* filename) {
- raw_log_fd = open(filename, O_WRONLY|O_CREAT, 00644);
+ raw_log_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 00644);
if (raw_log_fd == -1) {
TRACE(TRACE_ERROR, "failed to open rawlog outfile (%s)\n", filename);
return;
}
}
-static void main_rawlog_dump(uint8_t data_valid) {
+//static void main_rawlog_dump(uint8_t data_valid) {
+static void main_rawlog_dump(struct AutopilotMessageVIUp* current_message) {
struct timespec now;
clock_gettime(TIMER, &now);
struct raw_log_entry e;
+ e.time = absTime(time_diff(now, start));
+ memcpy(&e.message, current_message, sizeof(*current_message));
+ write(raw_log_fd, &e, sizeof(e));
+ /*struct raw_log_entry e;
+
e.time = absTime(time_diff(now, start));
+ e.message = current_message;
RATES_COPY(e.gyro, imu_float.gyro);
VECT3_COPY(e.accel, imu_float.accel);
VECT3_COPY(e.mag, imu_float.mag);
@@ -301,7 +312,7 @@
e.pressure_absolute = baro_measurement;
e.data_valid = data_valid;
write(raw_log_fd, &e, sizeof(e));
-
+ */
}
#if RUN_FILTER
Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
2010-10-26 16:26:41 UTC (rev 6272)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/test_libeknav_4.hpp
2010-10-26 17:09:15 UTC (rev 6273)
@@ -115,7 +115,8 @@
static void main_trick_libevent(void);
static void on_foo_event(int fd, short event __attribute__((unused)), void
*arg);
static struct event foo_event;
-static uint8_t main_dialog_with_io_proc(void);
+//static uint8_t main_dialog_with_io_proc(void);
+static void main_dialog_with_io_proc(void);
/* libeknav */
@@ -127,7 +128,8 @@
/* Logging */
#define IMU_LOG_FILE "/tmp/log_test3.bin"
static void main_rawlog_init(const char* filename);
-static void main_rawlog_dump(uint8_t);
+//static void main_rawlog_dump(uint8_t);
+static void main_rawlog_dump(struct AutopilotMessageVIUp* );
#if RUN_FILTER
static void print_estimator_state(double);
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6273] everything seems to work for the Kalman filter,
Martin Dieblich <=