From f2a8ca9e142dd5eef1f9029d6cdbf363085fcea0 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Mon, 18 Dec 2006 00:44:56 +0000 Subject: [PATCH] *** empty log message *** --- sw/logalizer/matlab/ahrs.m | 6 +++--- sw/logalizer/matlab/run_ahrs_on_log.m | 6 ++---- sw/logalizer/matlab/run_tilt.m | 14 ++++++++------ sw/logalizer/matlab/tilt.m | 27 +++++++++++++++++---------- 4 files changed, 30 insertions(+), 23 deletions(-) diff --git a/sw/logalizer/matlab/ahrs.m b/sw/logalizer/matlab/ahrs.m index 5bc39ba068..010b411656 100644 --- a/sw/logalizer/matlab/ahrs.m +++ b/sw/logalizer/matlab/ahrs.m @@ -117,14 +117,14 @@ quat_out = quat_in + quat_dot * dt; quat_out = normalize_quat(quat_out); -% A is the Jacobian of Xdot with respect to the states +% F is the Jacobian of Xdot with respect to the states q0 = quat_out(1); q1 = quat_out(2); q2 = quat_out(3); q3 = quat_out(4); -A = 0.5 * [ 0 -p -q -r q1 q2 q3 +F = 0.5 * [ 0 -p -q -r q1 q2 q3 p 0 r -q -q0 q3 -q2 q -r 0 p -q3 -q0 q1 r q -p 0 q2 -q1 -q0 @@ -132,7 +132,7 @@ A = 0.5 * [ 0 -p -q -r q1 q2 q3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ]; -P_dot = A * P_in + P_in * A' + Q_in; +P_dot = F * P_in + P_in * F' + Q_in; P_out = P_in + P_dot * dt; diff --git a/sw/logalizer/matlab/run_ahrs_on_log.m b/sw/logalizer/matlab/run_ahrs_on_log.m index 4b09be3058..91a0467cb0 100644 --- a/sw/logalizer/matlab/run_ahrs_on_log.m +++ b/sw/logalizer/matlab/run_ahrs_on_log.m @@ -1,10 +1,8 @@ clear -log = 'data/log_ahrs_bug'; -%log = 'data/log_imu_still'; -%log = 'data/log_imu_roll'; +%log = 'data/log_ahrs_bug'; %log = 'data/log_ahrs_still'; -%log = 'data/log_ahrs_roll'; +log = 'data/log_ahrs_roll'; %log = 'data/log_ahrs_yaw'; %log = 'data/log_ahrs_yaw_pitched'; diff --git a/sw/logalizer/matlab/run_tilt.m b/sw/logalizer/matlab/run_tilt.m index 6e307d362f..3a321ccdba 100644 --- a/sw/logalizer/matlab/run_tilt.m +++ b/sw/logalizer/matlab/run_tilt.m @@ -2,23 +2,24 @@ clear dt = 0.015625; -use_log = 0; +use_log = 1; if (use_log) - %log = 'data/log_ahrs_bug'; - log = 'data/log_ahrs_still'; + log = 'data/log_ahrs_bug'; + %log = 'data/log_ahrs_still'; %log = 'data/log_ahrs_roll'; %log = 'data/log_ahrs_yaw'; %log = 'data/log_ahrs_yaw_pitched'; [gyro, accel, mag] = read_imu_log(log); t = 0:dt:(length(gyro)-1)*dt; else - nb_samples = 3500; + nb_samples = 1500; [t, rates, quat] = synth_data(dt, nb_samples); [gyro, accel, mag] = synth_imu(rates, quat); end -tilt_status = 0; % uninit +% initialisation +tilt_status = 0; nb_init = 180; m_gyro = [ mean(gyro(1, 1:nb_init)) mean(gyro(2, 1:nb_init)) @@ -26,7 +27,7 @@ m_gyro = [ mean(gyro(1, 1:nb_init)) m_accel = [ mean(accel(1, 1:nb_init)) mean(accel(2, 1:nb_init)) mean(accel(3, 1:nb_init)) ]; -%[angle, bias] = + tilt(tilt_status, m_gyro, m_accel); [n, m] = size(gyro); @@ -39,6 +40,7 @@ for idx = 1:m end [theta_est(idx), bias(idx)] = tilt(tilt_status, gyro(:,idx), accel(:,idx)); theta_measure(idx) = theta_of_accel(accel(:,idx)); +% theta_measure(idx) = phi_of_accel(accel(:,idx)); end; subplot(3,1,1) diff --git a/sw/logalizer/matlab/tilt.m b/sw/logalizer/matlab/tilt.m index e6f28eee25..d4384aa0b3 100644 --- a/sw/logalizer/matlab/tilt.m +++ b/sw/logalizer/matlab/tilt.m @@ -1,3 +1,9 @@ +% +% this is a 2 states kalman filter used to fuse the readings of a +% two axis accelerometer and one axis gyro. +% The filter estimates the angle and the gyro bias. +% +% function [angle, bias, rate] = tilt(status, gyro, accel) @@ -10,23 +16,21 @@ persistent tilt_bias; % persistent tilt_rate; % unbiased rate persistent tilt_P; % covariance matrix -tilt_dt = 0.015625; % time step +tilt_dt = 0.015625; % prediction time step tilt_R = 0.3; % measurement covariance noise % means we expect a 0.3 rad jitter from the % accelerometer - - if (status == TILT_UNINIT) [tilt_angle, tilt_bias, tilt_rate, tilt_P] = tilt_init(gyro, accel); else - [tilt_angle, tilt_rate, tilt_P] = tilt_predict(gyro, tilt_P, ... - tilt_angle, tilt_bias, ... - tilt_dt); + [tilt_angle, tilt_rate, tilt_P] = ... + tilt_predict(gyro, tilt_P, ... + tilt_angle, tilt_bias, tilt_rate, ... + tilt_dt); if (status == TILT_UPDATE) [tilt_angle, tilt_bias, tilt_P] = tilt_update(accel, tilt_R, tilt_P, ... - tilt_angle, ... - tilt_bias); + tilt_angle, tilt_bias); end end @@ -42,6 +46,7 @@ rate = tilt_rate; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [angle, bias, rate, P] = tilt_init(gyro, accel) angle = theta_of_accel(accel); +%angle = phi_of_accel(accel); bias = gyro(2); @@ -59,12 +64,13 @@ P = [ 1 0 % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [angle_out, rate_out, P_out] = tilt_predict(gyro, P_in, ... - angle_in, bias, ... + angle_in, bias, rate,... dt) rate_out = gyro(2) - bias; +%rate_out = gyro(1) - bias; % update state ( X += Xdot * dt ) -angle_out = angle_in + rate_out * dt; +angle_out = angle_in + (rate + rate_out) / 2 * dt; % update covariance ( Pdot = A*P + P*A' + Q ) % ( P += Pdot * dt ) @@ -98,6 +104,7 @@ function [angle_out, bias_out, P_out] = tilt_update(accel, R, P_in, ... bias_in) measure_angle = theta_of_accel(accel); +%measure_angle = phi_of_accel(accel); err = measure_angle - angle_in; C = [ 1 0 ];