/* * attitude_bm.c * * Created on: 21.12.2010 * Author: Laurens Mackay, Tobias Naegeli */ #include #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.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0 , 0 , 0.001f , 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) { // 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 }; // XXX Hack - stop updating accel if upside down if (accel->z > 0) { mask[0] = 0.0f; mask[1] = 0.0f; mask[2] = 0.0f; } else { mask[0] = 1.0f; mask[1] = 1.0f; mask[2] = 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); }