diff options
Diffstat (limited to 'apps/px4/attitude_estimator_bm/attitude_bm.c')
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_bm.c | 314 |
1 files changed, 314 insertions, 0 deletions
diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c new file mode 100644 index 000000000..41ef47918 --- /dev/null +++ b/apps/px4/attitude_estimator_bm/attitude_bm.c @@ -0,0 +1,314 @@ + +/* + * attitude_bm.c + * + * Created on: 21.12.2010 + * Author: Laurens Mackay, Tobias Naegeli + */ + +#include <math.h> +#include "attitude_bm.h" +#include "kalman.h" + + +#define TIME_STEP (1.0f / 500.0f) + +static kalman_t attitude_blackmagic_kal; + +void vect_norm(float_vect3 *vect) +{ + float length = sqrtf( + vect->x * vect->x + vect->y * vect->y + vect->z * vect->z); + + if (length != 0) { + vect->x /= length; + vect->y /= length; + vect->z /= length; + } +} + + +void vect_cross_product(const float_vect3 *a, const float_vect3 *b, + float_vect3 *c) +{ + c->x = a->y * b->z - a->z * b->y; + c->y = a->z * b->x - a->x * b->z; + c->z = a->x * b->y - a->y * b->x; +} + +void attitude_blackmagic_update_a(void) +{ + // for acc + // Idendity matrix already in A. + M(attitude_blackmagic_kal.a, 0, 1) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 0, 2) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + + M(attitude_blackmagic_kal.a, 1, 0) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 1, 2) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + M(attitude_blackmagic_kal.a, 2, 0) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + M(attitude_blackmagic_kal.a, 2, 1) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + // for mag + // Idendity matrix already in A. + M(attitude_blackmagic_kal.a, 3, 4) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 3, 5) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + + M(attitude_blackmagic_kal.a, 4, 3) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 11); + M(attitude_blackmagic_kal.a, 4, 5) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + + M(attitude_blackmagic_kal.a, 5, 3) = TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 10); + M(attitude_blackmagic_kal.a, 5, 4) = -TIME_STEP * kalman_get_state( + &attitude_blackmagic_kal, 9); + +} + +void attitude_blackmagic_init(void) +{ + //X Kalmanfilter + //initalize matrices + + static m_elem kal_a[12 * 12] = { + 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0f + }; + + static m_elem kal_c[9 * 12] = { + 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 1.0f, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 1.0f, 0, 0, 1.0f + }; + + + +#define FACTOR 0.5 +#define FACTORstart 1 + + +// static m_elem kal_gain[12 * 9] = +// { 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0.004 , 0 , 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0 , 0, +// 0 , 0 , 0 , 0 , 0 , 0.015, 0 , 0 , 0, +// 0.0000 , +0.00002,0 , 0 , 0, 0, 0, 0 , 0, +// -0.00002,0 , 0 , 0 , 0, 0, 0, 0, 0, +// 0, 0 , 0 , 0, 0, 0, 0, 0, 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0 , 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 , 0, +// 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4 +// }; + + static m_elem kal_gain[12 * 9] = { + 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, + 0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0, + -0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0, + 0, 0 , 0 , 0, 0, 0, 0, 0, 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0 , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0, + 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f + }; + //offset update only correct if not upside down. + +#define K (10.0f*TIME_STEP) + + static m_elem kal_gain_start[12 * 9] = { + K, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, K, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, K, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, K, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, K, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, K, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, K, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, K, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, K, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0, + + 0, 0, 0, 0, 0, 0, 0, 0, 0 + }; + + + + static m_elem kal_x_apriori[12 * 1] = + { }; + + + //---> initial states sind aposteriori!? ---> fehler + static m_elem kal_x_aposteriori[12 * 1] = + { 0.0f, 0.0f, -1.0f, 0.6f, 0.0f, 0.8f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; + + kalman_init(&attitude_blackmagic_kal, 12, 9, kal_a, kal_c, + kal_gain_start, kal_gain, kal_x_apriori, kal_x_aposteriori, 1000); + +} + +void attitude_blackmagic(const float_vect3 *accel, const float_vect3 *mag, const float_vect3 *gyro) +{ + //Transform accelerometer used in all directions + // float_vect3 acc_nav; + //body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav); + + // Kalman Filter + + //Calculate new linearized A matrix + attitude_blackmagic_update_a(); + + kalman_predict(&attitude_blackmagic_kal); + + //correction update + + m_elem measurement[9] = + { }; + m_elem mask[9] = + { 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f }; + + measurement[0] = accel->x; + measurement[1] = accel->y; + measurement[2] = accel->z; + + measurement[3] = mag->x; + measurement[4] = mag->y; + measurement[5] = mag->z; + + measurement[6] = gyro->x; + measurement[7] = gyro->y; + measurement[8] = gyro->z; + + //Put measurements into filter + + +// static int j = 0; +// if (j >= 3) +// { +// j = 0; +// +// mask[3]=1; +// mask[4]=1; +// mask[5]=1; +// j=0; +// +// }else{ +// j++;} + + kalman_correct(&attitude_blackmagic_kal, measurement, mask); + +} +void attitude_blackmagic_get_all(float_vect3 *euler, float_vect3 *rates, float_vect3 *x_n_b, float_vect3 *y_n_b, float_vect3 *z_n_b) +{ + //debug + + // save outputs + float_vect3 kal_acc; + float_vect3 kal_mag; +// float_vect3 kal_w0, kal_w; + + kal_acc.x = kalman_get_state(&attitude_blackmagic_kal, 0); + kal_acc.y = kalman_get_state(&attitude_blackmagic_kal, 1); + kal_acc.z = kalman_get_state(&attitude_blackmagic_kal, 2); + + kal_mag.x = kalman_get_state(&attitude_blackmagic_kal, 3); + kal_mag.y = kalman_get_state(&attitude_blackmagic_kal, 4); + kal_mag.z = kalman_get_state(&attitude_blackmagic_kal, 5); + +// kal_w0.x = kalman_get_state(&attitude_blackmagic_kal, 6); +// kal_w0.y = kalman_get_state(&attitude_blackmagic_kal, 7); +// kal_w0.z = kalman_get_state(&attitude_blackmagic_kal, 8); +// +// kal_w.x = kalman_get_state(&attitude_blackmagic_kal, 9); +// kal_w.y = kalman_get_state(&attitude_blackmagic_kal, 10); +// kal_w.z = kalman_get_state(&attitude_blackmagic_kal, 11); + + rates->x = kalman_get_state(&attitude_blackmagic_kal, 9); + rates->y = kalman_get_state(&attitude_blackmagic_kal, 10); + rates->z = kalman_get_state(&attitude_blackmagic_kal, 11); + + + +// kal_w = kal_w; // XXX hack to silence compiler warning +// kal_w0 = kal_w0; // XXX hack to silence compiler warning + + + + //debug_vect("magn", mag); + + //float_vect3 x_n_b, y_n_b, z_n_b; + z_n_b->x = -kal_acc.x; + z_n_b->y = -kal_acc.y; + z_n_b->z = -kal_acc.z; + vect_norm(z_n_b); + vect_cross_product(z_n_b, &kal_mag, y_n_b); + vect_norm(y_n_b); + + vect_cross_product(y_n_b, z_n_b, x_n_b); + + + + //save euler angles + euler->x = atan2f(z_n_b->y, z_n_b->z); + euler->y = -asinf(z_n_b->x); + euler->z = atan2f(y_n_b->x, x_n_b->x); + +} |