void kalman_gravity_demo()
{
// initialize the filter
kalman_gravity_init();
mf16 *x = kalman_get_state_vector(&kf);
mf16 *z = kalman_get_observation_vector(&kfm);
// filter!
uint_fast16_t i;
for (i = 0; i < MEAS_COUNT; ++i)
{
// prediction.
kalman_predict(&kf);
// measure ...
fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]);
matrix_set(z, 0, 0, measurement);
// update
kalman_correct(&kf, &kfm);
}
// fetch estimated g
const fix16_t g_estimated = x->data[2][0];
const float value = fix16_to_float(g_estimated);
assert(value > 9.7 && value < 10);
}
/*
* executes kalman predict and correct step
*/
void kalman_run(kalman_out_t *out, kalman_t *kalman, const kalman_in_t *in)
{
/* A = | init dt |
| init init | */
m_set_val(kalman->A, 0, 1, in->dt);
/* B = | 0.5 * dt ^ 2 |
| dt | */
m_set_val(kalman->B, 0, 0, 0.5 * in->dt * in->dt);
m_set_val(kalman->B, 1, 0, in->dt);
kalman_predict(kalman, in->acc);
kalman_correct(kalman, in->pos, 0.0);
out->pos = v_entry(kalman->x, 0);
out->speed = v_entry(kalman->x, 1);
}
/*
* executes kalman predict and correct step
*/
static void kalman_run(kalman_t *kf, float *est_pos, float *est_speed, float pos, float speed, float acc, float dt)
{
/* A = | init dt |
| init init | */
kf->A.me[0][1] = dt;
/* B = | 0.5 * dt ^ 2 |
| dt | */
kf->B.me[0][0] = 0.5f * dt * dt;
kf->B.me[1][0] = dt;
/* Q, R: */
mat_scalar_mul(&kf->Q, &kf->I, tsfloat_get(kf->q));
mat_scalar_mul(&kf->R, &kf->I, tsfloat_get(kf->r));
kalman_predict(kf, acc);
kalman_correct(kf, pos, speed);
*est_pos = kf->x.ve[0];
*est_speed = kf->x.ve[1];
}
void attitude_tobi_laurens(void)
{
//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_tobi_laurens_update_a();
kalman_predict(&attitude_tobi_laurens_kal);
//correction update
m_elem measurement[9] =
{ };
m_elem mask[9] =
{ 1.0f, 1.0f, 1.0f, 0, 0, 0, 1.0f, 1.0f, 1.0f };
float_vect3 acc;
float_vect3 mag;
float_vect3 gyro;
// acc.x = global_data.accel_raw.x * 9.81f /690;
// acc.y = global_data.accel_raw.y * 9.81f / 690;
// acc.z = global_data.accel_raw.z * 9.81f / 690;
#ifdef IMU_PIXHAWK_V250
acc.x = global_data.accel_raw.x * (650.0f/900.0f);
acc.y = global_data.accel_raw.y * (650.0f/900.0f);
acc.z = global_data.accel_raw.z * (650.0f/900.0f);
#else
acc.x = global_data.accel_raw.x;
acc.y = global_data.accel_raw.y;
acc.z = global_data.accel_raw.z;
#endif
float acc_norm = sqrt(global_data.accel_raw.x * global_data.accel_raw.x + global_data.accel_raw.y * global_data.accel_raw.y + global_data.accel_raw.z * global_data.accel_raw.z);
static float acc_norm_filt = SCA3100_COUNTS_PER_G;
float acc_norm_lp = 0.05f;
acc_norm_filt = (1.0f - acc_norm_lp) * acc_norm_filt + acc_norm_lp
* acc_norm;
// static float acc_norm_filtz = SCA3100_COUNTS_PER_G;
// float acc_norm_lpz = 0.05;
// acc_norm_filtz = (1.0f - acc_norm_lpz) * acc_norm_filtz + acc_norm_lpz * -acc.z;
float acc_diff = fabs(acc_norm_filt - SCA3100_COUNTS_PER_G);
if (acc_diff > 200.0f)
{
//Don't correct when acc high
mask[0] = 0;
mask[1] = 0;
mask[2] = 0;
}
else if (acc_diff > 100.0f)
{
//fade linearely out between 100 and 200
float mask_lin = (200.0f - acc_diff) / 100.0f;
mask[0] = mask_lin;
mask[1] = mask_lin;
mask[2] = mask_lin;
}
//else mask stays 1
// static uint8_t i = 0;
// if (i++ > 10)
// {
// i = 0;
// float_vect3 debug;
// debug.x = mask[0];
// debug.y = acc_norm;
// debug.z = acc_norm_filt;
// debug_vect("acc_norm", debug);
// }
// mag.x = (global_data.magnet_corrected.x ) * 1.f / 510.f;
// mag.y = (global_data.magnet_corrected.y) * 1.f / 510.f;
// mag.z = (global_data.magnet_corrected.z) * 1.f / 510.f;
mag.x = (global_data.magnet_corrected.x ) ;
mag.y = (global_data.magnet_corrected.y) ;
mag.z = (global_data.magnet_corrected.z) ;
// gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.000955;
// gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.000955;
// gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010;
gyro.x = -(global_data.gyros_raw.x-global_data.param[PARAM_GYRO_OFFSET_X]) * 0.001008f;
gyro.y = (global_data.gyros_raw.y-global_data.param[PARAM_GYRO_OFFSET_Y]) * 0.001008f;
gyro.z = -(global_data.gyros_raw.z-global_data.param[PARAM_GYRO_OFFSET_Z]) * 0.001010f;
measurement[0] = acc.x;
measurement[1] = acc.y;
measurement[2] = acc.z;
if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION)
{
measurement[3] = global_data.vision_magnetometer_replacement.x;
measurement[4] = global_data.vision_magnetometer_replacement.y;
measurement[5] = global_data.vision_magnetometer_replacement.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
{
// measurement[3] = global_data.vision_global_magnetometer_replacement.x;
// measurement[4] = global_data.vision_global_magnetometer_replacement.y;
// measurement[5] = global_data.vision_global_magnetometer_replacement.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON)
{
measurement[3] = global_data.vicon_magnetometer_replacement.x;
measurement[4] = global_data.vicon_magnetometer_replacement.y;
measurement[5] = global_data.vicon_magnetometer_replacement.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
{
// Do nothing
// KEEP THIS IN HERE
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_MAGNETOMETER) // YAW_ESTIMATION_MODE_MAGNETOMETER
{
measurement[3] = mag.x;
measurement[4] = mag.y;
measurement[5] = mag.z;
// debug_vect("mag_f", mag);
}
else
{
static uint8_t errcount = 0;
if (errcount == 0) debug_message_buffer("ATT EST. ERROR: No valid yaw estimation mode set!");
errcount++;
global_data.state.status = MAV_STATE_CRITICAL;
}
measurement[6] = gyro.x;
measurement[7] = gyro.y;
measurement[8] = gyro.z;
//Put measurements into filter
static int j = 0;
// MASK
if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
{
// Do nothing, pure integration
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VICON)
{
// If our iteration count is right and new vicon data is available, update measurement
if (j >= 3 && global_data.state.vicon_attitude_new_data == 1)
{
j = 0;
mask[3]=1;
mask[4]=1;
mask[5]=1;
}
else
{
j++;
}
global_data.state.vicon_attitude_new_data = 0;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_VISION)
{
// If the iteration count is right and new vision data is available, update measurement
if (j >= 3 && global_data.state.vision_attitude_new_data == 1)
{
j = 0;
mask[3]=1;
mask[4]=1;
mask[5]=1;
}
else
{
j++;
}
global_data.state.vision_attitude_new_data = 0;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
{
// // If the iteration count is right and new vision data is available, update measurement
// if (j >= 3 && global_data.state.global_vision_attitude_new_data == 1)
// {
// j = 0;
//
// mask[3]=1;
// mask[4]=1;
// mask[5]=1;
// j=0;
// }
// else
// {
// j++;
// }
// global_data.state.global_vision_attitude_new_data = 0;
}
else
{
if (j >= 3 && global_data.state.magnet_ok)
{
j = 0;
mask[3]=1;
mask[4]=1;
mask[5]=1;
}
else
{
j++;
}
}
kalman_correct(&attitude_tobi_laurens_kal, measurement, mask);
//debug
// save outputs
float_vect3 kal_acc, kal_mag, kal_w0, kal_w;
kal_acc.x = kalman_get_state(&attitude_tobi_laurens_kal, 0);
kal_acc.y = kalman_get_state(&attitude_tobi_laurens_kal, 1);
kal_acc.z = kalman_get_state(&attitude_tobi_laurens_kal, 2);
kal_mag.x = kalman_get_state(&attitude_tobi_laurens_kal, 3);
kal_mag.y = kalman_get_state(&attitude_tobi_laurens_kal, 4);
kal_mag.z = kalman_get_state(&attitude_tobi_laurens_kal, 5);
kal_w0.x = kalman_get_state(&attitude_tobi_laurens_kal, 6);
kal_w0.y = kalman_get_state(&attitude_tobi_laurens_kal, 7);
kal_w0.z = kalman_get_state(&attitude_tobi_laurens_kal, 8);
kal_w.x = kalman_get_state(&attitude_tobi_laurens_kal, 9);
kal_w.y = kalman_get_state(&attitude_tobi_laurens_kal, 10);
kal_w.z = kalman_get_state(&attitude_tobi_laurens_kal, 11);
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
global_data.attitude.x = atan2f(z_n_b.y, z_n_b.z);
global_data.attitude.y = -asinf(z_n_b.x);
if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_INTEGRATION)
{
global_data.attitude.z += 0.005f * global_data.gyros_si.z;
}
else if (global_data.state.yaw_estimation_mode == YAW_ESTIMATION_MODE_GLOBAL_VISION)
{
global_data.attitude.z += 0.0049f * global_data.gyros_si.z;
if (global_data.state.global_vision_attitude_new_data == 1)
{
global_data.attitude.z = 0.995f*global_data.attitude.z + 0.005f*global_data.vision_data_global.ang.z;
// Reset new data flag at roughly 1 Hz, detecting a vision timeout
static uint8_t new_data_reset = 0;
if (new_data_reset == 0)
{
global_data.state.global_vision_attitude_new_data = 0;
}
new_data_reset++;
}
}
else
{
// static hackMagLowpass = 0.0f;
global_data.attitude.z = global_data.attitude.z*0.9f+0.1f*atan2f(y_n_b.x, x_n_b.x);
}
//save rates
global_data.attitude_rate.x = kal_w.x;
global_data.attitude_rate.y = kal_w.y;
global_data.attitude_rate.z = kal_w.z;
global_data.yaw_lowpass = 0.99f * global_data.yaw_lowpass + 0.01f * global_data.attitude_rate.z;
}
void optflow_speed_kalman(void)
{
//Transform accelerometer used in all directions
float_vect3 acc_nav;
body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav);
// //Calculate gyro offsets. Workaround for old attitude filter.
// static float gyro_x_offset = 0, gyro_y_offset = 0;
// float lp = 0.001f;
// gyro_x_offset = (1 - lp) * gyro_x_offset + lp * global_data.gyros_si.x;
// gyro_y_offset = (1 - lp) * gyro_y_offset + lp * global_data.gyros_si.y;
//Low-pass filter for sonar with all spikes. Makes filter following big steps.
static float sonar_distance_spike = 0;
float sonar_distance_spike_lp = 0.1; // ~ 1/time to get to new step
sonar_distance_spike = (1 - sonar_distance_spike_lp) * sonar_distance_spike + sonar_distance_spike_lp * global_data.sonar_distance;
//Low-pass filter for sonar without spikes
//only update this low-pass if the signal is close to one of these two low-pass filters.
static float sonar_distance = 0;
float z_lp = 0.2; // real low-pass on spike rejected data.
float spike_reject_threshold = 0.2f; // 0.4 m
uint8_t sonar_distance_rejecting_spike=0;
if ((fabs(sonar_distance_spike - global_data.sonar_distance) < spike_reject_threshold) ||
(fabs(sonar_distance - global_data.sonar_distance) < spike_reject_threshold))
{
sonar_distance = (1 - z_lp) * sonar_distance + z_lp
* global_data.sonar_distance * cos(global_data.attitude.x)
* cos(global_data.attitude.y);
}
else
{
sonar_distance_rejecting_spike = 1;
}
global_data.sonar_distance_filtered = sonar_distance;
#ifdef IMU_PIXHAWK_V260_EXTERNAL_MAG
//pressure altitude
//Altitude Kalman Filter
kalman_predict(&outdoor_position_kalman_z);
m_elem z_measurement[2] =
{ };
m_elem z_mask[2] =
{ 0, 1 };//we normaly only have acceleration an no pressure measurement
//prepare measurement data
//measurement #1 pressure => relative altitude
//sensors_pressure_bmp085_read_out();
if (global_data.state.pressure_ok)
{
z_measurement[0] = -calc_altitude_pressure(global_data.pressure_raw);
z_mask[0] = 1;//we have a pressure measurement to update
//debug output
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 50,
// outdoor_position_kalman_z.gainfactor);
}
//measurement #2 acceleration
z_measurement[1] = acc_nav.z;
//Put measurements into filter
kalman_correct(&outdoor_position_kalman_z, z_measurement, z_mask);
//use results
float sonar_distance_use_treshold = 2.0f;//m
static float ground_altitude = -0.0f;
if (sonar_distance >= sonar_distance_use_treshold
|| sonar_distance_rejecting_spike)
{
global_data.position.z
= kalman_get_state(&outdoor_position_kalman_z, 0)
- ground_altitude;
}
else
{
global_data.position.z = -sonar_distance;
ground_altitude = kalman_get_state(&outdoor_position_kalman_z, 0)
- (-sonar_distance);
}
//use velocity always
global_data.velocity.z = kalman_get_state(&outdoor_position_kalman_z, 1);
#endif
// transform optical flow into global frame
float_vect3 flow, flowQuad, flowWorld;//, flowQuadUncorr, flowWorldUncorr;
flow.x = global_data.optflow.x;
flow.y = global_data.optflow.y;
flow.z = 0.0;
turn_xy_plane(&flow, PI, &flowQuad);
flowQuad.x = flowQuad.x * scale - global_data.attitude_rate.y;
flowQuad.y = flowQuad.y * scale + global_data.attitude_rate.x;
// This was for biased gyro rates
// flowQuad.x = flowQuad.x * scale - (global_data.gyros_si.y - gyro_y_offset);
// flowQuad.y = flowQuad.y * scale + (global_data.gyros_si.x - gyro_x_offset);
body2navi(&flowQuad, &global_data.attitude, &flowWorld);
// turn_xy_plane(&flow, PI, &flowQuadUncorr);
// body2navi(&flowQuadUncorr, &global_data.attitude, &flowWorldUncorr);
//distance from flow sensor to ground
//float flow_distance = -global_data.vicon_data.z;
float flow_distance = sonar_distance;
static float px = 0.0;
static float py = 0.0;
float QxLocal = 0.1, QyLocal = 0.1;
//float RxLocal = 0.1, RyLocal = 0.1;
// initializes x and y to global position
if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_VICON)
{
global_data.position.x = global_data.vicon_data.x;
global_data.position.y = global_data.vicon_data.y;
}
else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_GLOBAL_VISION && global_data.vision_data_global.new_data == 1)
{
// global_data.position.x = global_data.position.x*0.6f + 0.4f*global_data.vision_data_global.pos.x;
// global_data.position.y = global_data.position.y*0.6f + 0.4f*global_data.vision_data_global.pos.y;
//simple 1D Kalman filtering (x direction)
float x_ = global_data.position.x;
float px_ = px + QxLocal;
float Kx = 0.4f;//px_/(px_ + RxLocal);
float x = x_ + Kx*(global_data.vision_data_global.pos.x - x_);
px = (1.0 - Kx)*px_;
global_data.position.x = x;
//simple 1D Kalman filtering (y direction)
float y_ = global_data.position.y;
float py_ = py + QyLocal;
float Ky = 0.4f;//py_/(py_ + RyLocal);
float y = y_ + Ky*(global_data.vision_data_global.pos.y - y_);
py = (1.0 - Ky)*py_;
global_data.position.y = y;
global_data.vision_data_global.new_data = 0;
}
else if (global_data.state.position_estimation_mode == POSITION_ESTIMATION_MODE_OPTICAL_FLOW_ULTRASONIC_NON_INTEGRATING)
{
global_data.position.x = 0;
global_data.position.y = 0;
global_data.position.z = 0;
}
// global_data.position.z = global_data.vicon_data.z;
//---------------------------------------------------
// Vx Kalman Filter
// prediction
float vx_ = ax * vx;
if (global_data.state.fly == FLY_FLYING)
{
vx += bx * global_data.attitude.y;
}
float pvx_ = ax * pvx + Qx;
// do an update only if optical flow is good
if (global_data.optflow.z > 10.0)
{
// kalman gain
float Kx = pvx_ * ax / (ax * pvx_ * ax + Rx);
// update step
//float xflow = global_data.optflow.x*global_data.position.z*scale;
float xflow = flow_distance * flowWorld.x;
vx = vx_ + Kx * (xflow - cx * vx_);
pvx = (1.0 - Kx * cx) * pvx_;
}
// otherwise take only the prediction
else
{
// Let speed decay to zero if no measurements are available
vx = vx_*0.95;
pvx = pvx_;
}
// assign readings from Kalman Filter
global_data.velocity.x = vx;
global_data.position.x += vx * VEL_KF_TIME_STEP_X;
//---------------------------------------------------
// Vy Kalman Filter
// prediction
float vy_ = ay * vy;
if (global_data.state.fly == FLY_FLYING)
{
vy_ += by * global_data.attitude.x;
}
float pvy_ = ay * pvy + Qy;
// do an update only if optical flow is good
if (global_data.optflow.z > 10.0)
{
// kalman gain
float Ky = pvy_ * ay / (ay * pvy_ * ay + Ry);
// update step
//float yflow = global_data.optflow.y*global_data.position.z*scale;
float yflow = flow_distance * flowWorld.y;
vy = vy_ + Ky * (yflow - cy * vy_);
pvy = (1.0 - Ky * cy) * pvy_;
}
// otherwise take only the prediction
else
{
// Let speed decay to zero if no measurements are available
vy = vy_*0.95;
pvy = pvy_;
}
// assign readings from Kalman Filter
global_data.velocity.y = vy;
global_data.position.y += vy * VEL_KF_TIME_STEP_Y;
// float_vect3 debug;
//debug.x = (global_data.gyros_si.x - gyro_x_offset);
//debug.y = (global_data.attitude_rate.x);
// debug.x =
// = sonar_distance;
// debug.x =- calc_altitude_pressure(global_data.pressure_raw);
// debug.y = sonar_distance_spike;
// debug.z = global_data.sonar_distance_filtered;
// debug.y=kalman_get_state(&outdoor_position_kalman_z, 0);
// debug.z=ground_altitude;
// debug_vect("altitude", debug);
}
void vision_position_kalman(void)
{
//Transform accelerometer used in all directions
float_vect3 acc_nav;
body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav);
//X &Y Kalman Filter
kalman_predict(&vision_position_kalman_x);
kalman_predict(&vision_position_kalman_y);
kalman_predict(&vision_position_kalman_z);
m_elem x_measurement[2] =
{ };
m_elem x_mask[2] =
{ 0, 0 };//only acceleromenters normaly
m_elem y_measurement[2] =
{ };
m_elem y_mask[2] =
{ 0, 0 };//only acceleromenters normaly
m_elem z_measurement[2] =
{ };
m_elem z_mask[2] =
{ 0, 0 };//only acceleromenters normaly
// static int i = 0;
// if (i++ == 200)
// {
// i = 0;
// x_measurement[0]=0;
// x_mask[0]=1;//simulate GPS position 0 at 1 Hz
// } static int i = 0;
if (global_data.vision_data.new_data || global_data.state.vicon_new_data)
{
//measure difference:
float difference = sqrtf((global_data.vision_data.pos.x
- global_data.vicon_data.x) * (global_data.vision_data.pos.x
- global_data.vicon_data.x) + (global_data.vision_data.pos.y
- global_data.vicon_data.y) * (global_data.vision_data.pos.y
- global_data.vicon_data.y) + (global_data.vision_data.pos.z
- global_data.vicon_data.z) * (global_data.vision_data.pos.z
- global_data.vicon_data.z));
//use only vision_data if difference to vicon is small or we don't have vicon_data at all
if (difference < global_data.param[PARAM_VICON_TAKEOVER_DISTANCE] || !global_data.state.vicon_ok)
{
if (global_data.vision_data.new_data)
{
//vision
x_measurement[0] = global_data.vision_data.pos.x;
x_mask[0] = 1;
y_measurement[0] = global_data.vision_data.pos.y;
y_mask[0] = 1;
z_measurement[0] = global_data.vision_data.pos.z;
z_mask[0] = 1;
}
}
else if (global_data.state.vicon_new_data)
{ //vicon
x_measurement[0] = global_data.vicon_data.x;
x_mask[0] = 1;
y_measurement[0] = global_data.vicon_data.y;
y_mask[0] = 1;
z_measurement[0] = global_data.vicon_data.z;
z_mask[0] = 1;
}
//data has been used
global_data.vision_data.new_data = 0;
global_data.state.vicon_new_data = 0;
if (global_data.vision_data.new_data)
{
// uint32_t vision_delay = (uint32_t) (global_data.vision_data.comp_end
// - global_data.vision_data.time_captured);
// // Debug Time for Vision Processing
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 100,
// (float) vision_delay);
}
}
x_measurement[1] = acc_nav.x;
y_measurement[1] = acc_nav.y;
z_measurement[1] = acc_nav.z;
//Put measurements into filter
kalman_correct(&vision_position_kalman_x, x_measurement, x_mask);
kalman_correct(&vision_position_kalman_y, y_measurement, y_mask);
kalman_correct(&vision_position_kalman_z, z_measurement, z_mask);
/* static int i=2;
if(i++==4){
i=0;
//debug
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 50,
// z_measurement[1]);
float_vect3 out_kal_z;
out_kal_z.x = kalman_get_state(&vision_position_kalman_z,1);
out_kal_z.y = kalman_get_state(&vision_position_kalman_z,2);
out_kal_z.z = kalman_get_state(&vision_position_kalman_z,3);
debug_vect("out_kal_z", out_kal_z);
}*/
// save outputs
global_data.position.x = kalman_get_state(&vision_position_kalman_x,0);
global_data.position.y = kalman_get_state(&vision_position_kalman_y,0);
global_data.position.z = kalman_get_state(&vision_position_kalman_z,0);
global_data.velocity.x = kalman_get_state(&vision_position_kalman_x,1);
global_data.velocity.y = kalman_get_state(&vision_position_kalman_y,1);
global_data.velocity.z = kalman_get_state(&vision_position_kalman_z,1);
}
void outdoor_position_kalman(void)
{
//Transform accelerometer used in all directions
float_vect3 acc_nav;
body2navi(&global_data.accel_si, &global_data.attitude, &acc_nav);
//X &Y Kalman Filter
kalman_predict(&outdoor_position_kalman_x);
kalman_predict(&outdoor_position_kalman_y);
m_elem x_measurement[2] =
{ };
m_elem x_mask[2] =
{ 0, 1 };//only acceleromenters normaly
m_elem y_measurement[2] =
{ };
m_elem y_mask[2] =
{ 0, 1 };//only acceleromenters normaly
// static int i = 0;
// if (i++ == 200)
// {
// i = 0;
// x_measurement[0]=0;
// x_mask[0]=1;//simulate GPS position 0 at 1 Hz
// } static int i = 0;
if (global_data.state.gps_ok && global_data.state.gps_new_data)
{
global_data.state.gps_new_data=0;
float_vect3 gps_local;
gps_get_local_position(&gps_local);
x_measurement[0] = gps_local.x;
x_mask[0] = 1;// GPS position at 1 Hz
y_measurement[0] = gps_local.y;
y_mask[0] = 1;// GPS position at 1 Hz
}
x_measurement[1] = acc_nav.x;
y_measurement[1] = acc_nav.y;
//Put measurements into filter
kalman_correct(&outdoor_position_kalman_x, x_measurement, x_mask);
kalman_correct(&outdoor_position_kalman_y, y_measurement, y_mask);
// static int i=2;
// if(i++==4){
// i=0;
//debug
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 50,
// x_measurement[1]);
// float_vect3 out_kal_x;
// out_kal_x.x = kalman_get_state(&outdoor_position_kalman_x,0);
// out_kal_x.y = kalman_get_state(&outdoor_position_kalman_x,1);
// out_kal_x.z = kalman_get_state(&outdoor_position_kalman_x,3);
// debug_vect("out_kal_x", out_kal_x);
//Altitude Kalman Filter
kalman_predict(&outdoor_position_kalman_z);
m_elem z_measurement[2] =
{ };
m_elem z_mask[2] =
{ 0, 1 };//we normaly only have acceleration an no pressure measurement
//prepare measurement data
//measurement #1 pressure => relative altitude
static int nopressure = 1;
nopressure++;
if (nopressure == 2 || nopressure == 4)
{
sensors_pressure_bmp085_read_out();
}
if (nopressure == 4)
{
nopressure = 0;
if (global_data.state.pressure_ok)
{
if (altitude_local_origin)
{
z_measurement[0] = -calc_altitude_pressure(
global_data.pressure_raw) - altitude_local_origin;
}
else
{
altitude_set_local_origin();
}
z_mask[0] = 1;//we have a pressure measurement to update
//debug output
// mavlink_msg_debug_send(global_data.param[PARAM_SEND_DEBUGCHAN], 50,
// outdoor_position_kalman_z.gainfactor);
}
}
//measurement #2 acceleration
z_measurement[1] = acc_nav.z;
//Put measurements into filter
kalman_correct(&outdoor_position_kalman_z, z_measurement, z_mask);
float_vect3 debug, debugv;
debug.x = kalman_get_state(&outdoor_position_kalman_x, 0);
debug.y = kalman_get_state(&outdoor_position_kalman_y, 0);
debug.z = kalman_get_state(&outdoor_position_kalman_z, 0);
outdoor_z_position = debug.z;
debugv.x = kalman_get_state(&outdoor_position_kalman_x, 1);
debugv.y = kalman_get_state(&outdoor_position_kalman_y, 1);
debugv.z = kalman_get_state(&outdoor_position_kalman_z, 1);
static uint8_t i;
if (i++ > 20)
{
i = 0;
debug_vect("outdoot_pos", debug);
debug_vect("outdoot_vel", debugv);
}
// save outputs
// global_data.position.x = kalman_get_state(&outdoor_position_kalman_x,0);
// global_data.position.y = kalman_get_state(&outdoor_position_kalman_y,0);
// global_data.position.z = kalman_get_state(&outdoor_position_kalman_z,0);
//
// global_data.velocity.x = kalman_get_state(&outdoor_position_kalman_x,1);
// global_data.velocity.y = kalman_get_state(&outdoor_position_kalman_y,1);
// global_data.velocity.z = kalman_get_state(&outdoor_position_kalman_z,1);
}
