int main()
{
RunningStat rs;
rs.Push(17.0);
rs.Push(19.0);
rs.Push(24.0);
double mean = rs.Mean();
double variance = rs.Variance();
double stdev = rs.StandardDeviation();
cout << "mean = " << mean << endl;
cout << "variance = " << variance << endl;
cout << "stdev = " << stdev << endl;
}
void SlipDetect::filter()
{
double odom_v, odom_w, imu_x, imu_y, imu_w, gps_vel_fix, gps_vel;
odom_v = last_odom_.twist.twist.linear.x;
odom_w = last_odom_.twist.twist.angular.z;
imu_x = last_imu_.linear_acceleration.x;
imu_y = last_imu_.linear_acceleration.y;
imu_w = last_imu_.angular_velocity.z * M_PI / 180;
gps_vel_fix = prev_gps_vel_fix_;
double x_accel, y_accel;
x_accel = imu_x - imu_stat_x_.Mean();
y_accel = imu_y - imu_stat_y_.Mean();
cutter_msgs::Testing test;
cutter_msgs::SlipStatus slip;
/*
// DONT WORRY ABOUT INITIALIZING CAUSE WE'RE NOT USING ACCELEROMETER
if (imu_new_ && !initialized_)
{
if (fabs(odom_v) < .001 && fabs(odom_w) < .001)
{
// Filter the IMU when the wheels report no motion (assume the robot is not moving)
imu_stat_x_.Push(imu_x);
imu_stat_y_.Push(imu_y);
ROS_INFO("Imu X: %f, X average: %f, Stddev: %f, X-avg: %f",
imu_x, imu_stat_x_.Mean(), imu_stat_x_.StandardDeviation(), imu_x-imu_stat_x_.Mean());
ROS_INFO("Imu Y: %f, Y average: %f, Stddev: %f, Y-avg: %f",
imu_y, imu_stat_y_.Mean(), imu_stat_y_.StandardDeviation(), imu_y-imu_stat_y_.Mean());
ROS_INFO("Odom V: %f, Odom W: %f, fabs(odomv): %f, fabs(odomw): %f", odom_v, odom_w, fabs(odom_v), fabs(odom_w));
if (initialized_cnt_++ > 50)
initialized_ = true;
ROS_INFO("Count: %i", initialized_cnt_);
}
else
{
// Currently do nothing with the IMU if we're moving??
ROS_INFO("imu: No motion");
}
} */
bool no_update = true;
if (odom_new_ && imu_new_)
{
no_update = false;
//central_filter_.addMeasurementEncoders(odom_v, odom_w);
//central_filter_.addMeasurementIMU(x_accel, imu_w);
//central_filter_.update();
// Update the Encoder Filter
kf_enc_.addMeasurementEncoders(odom_v, odom_w);
kf_enc_.update();
// Update the Auxillary Filter
kf_aux_.addMeasurementIMU(x_accel, imu_w); // Note x_accel not used in the kalman filter
if (gps_new_ && use_gps_)
{
if (!leverarm_valid_)
{
getGPSLeverarm();
}
else
{
double xoff, yoff, gps_dist;
xoff = new_gps_.x - old_gps_.x;
yoff = new_gps_.y - old_gps_.y;
// GpsDist = new gps - old gps
gps_dist = sqrt( xoff*xoff + yoff*yoff );
// GpsVel = GpsDist / 0.1 <--- Distance / GPS DT where GPS DT = 10 Hz or 0.1 s
gps_vel = gps_dist / 0.1;
// GpsVelFix = sqrt( GpsVel^2 - (r*w)^2 ) <--- Ie, subtract out component of motion due to angular velocity
gps_vel_fix = sqrt( gps_vel*gps_vel - gps_leverarm_*gps_leverarm_*imu_w*imu_w );
if (odom_v < 0)
gps_vel_fix = - gps_vel_fix;
ROS_INFO("GPS Velocity: %f, Odom Velocity: %f", gps_vel_fix, odom_v);
prev_gps_vel_fix_ = gps_vel_fix;
kf_aux_.addMeasurementGPS(gps_vel_fix);
gps_new_ = false;
}
}
kf_aux_.update();
// Fuse the two Filters
double Ptot_v, Ptot_w, Ptot_a, Ptot_wdot;
double Xtot_v, Xtot_w, Xtot_a, Xtot_wdot;
Ptot_v = kf_enc_.cov_[0] * kf_aux_.cov_[0] / (kf_enc_.cov_[0] + kf_aux_.cov_[0]);
Ptot_w = kf_enc_.cov_[5] * kf_aux_.cov_[5] / (kf_enc_.cov_[5] + kf_aux_.cov_[5]);
Ptot_a = kf_enc_.cov_[10] * kf_aux_.cov_[10] / (kf_enc_.cov_[10] + kf_aux_.cov_[10]);
Ptot_wdot = kf_enc_.cov_[15] * kf_aux_.cov_[15] / (kf_enc_.cov_[15] + kf_aux_.cov_[15]);
Xtot_v = Ptot_v / kf_enc_.cov_[0] * kf_enc_.state_[0] + Ptot_v / kf_aux_.cov_[0] * kf_aux_.state_[0];
Xtot_w = Ptot_w / kf_enc_.cov_[5] * kf_enc_.state_[1] + Ptot_w / kf_aux_.cov_[5] * kf_aux_.state_[1];
Xtot_a = Ptot_a / kf_enc_.cov_[10] * kf_enc_.state_[2] + Ptot_a / kf_aux_.cov_[10] * kf_aux_.state_[2];
Xtot_wdot = Ptot_wdot / kf_enc_.cov_[15] * kf_enc_.state_[3] + Ptot_wdot / kf_aux_.cov_[15] * kf_aux_.state_[3];
ROS_INFO("Enc Cov Diag: [ %f, %f, %f, %f]", kf_enc_.cov_[0], kf_enc_.cov_[5], kf_enc_.cov_[10], kf_enc_.cov_[15]);
ROS_INFO("Enc State: [ %f, %f, %f, %f]", kf_enc_.state_[0], kf_enc_.state_[1], kf_enc_.state_[2], kf_enc_.state_[3]);
ROS_INFO("Aux Cov Diag: [ %f, %f, %f, %f]", kf_aux_.cov_[0], kf_aux_.cov_[5], kf_aux_.cov_[10], kf_aux_.cov_[15]);
ROS_INFO("Aux State: [ %f, %f, %f, %f]", kf_aux_.state_[0], kf_aux_.state_[1], kf_aux_.state_[2], kf_aux_.state_[3]);
ROS_INFO("Tot Cov Diag: [ %f, %f, %f, %f]", Ptot_v, Ptot_w, Ptot_a, Ptot_wdot);
ROS_INFO("Tot State: [ %f, %f, %f, %f]", Xtot_v, Xtot_w, Xtot_a, Xtot_wdot);
//ROS_INFO("P_v_enc: %f, P_v_aux: %f, P_a_enc: %f, P_a_aux: %f", Ptot_v / kf_enc_.cov_[0], Ptot_v / kf_aux_.cov_[0], Ptot_a / kf_enc_.cov_[10], Ptot_a / kf_aux_.cov_[10]);
// Save off all the debugging info I want
test.corrected_imu_x = x_accel;
test.corrected_imu_y = y_accel;
test.kf_v = Xtot_v;
test.kf_w = Xtot_w;
test.kf_a = Xtot_a;
test.kf_wdot = Xtot_wdot;
test.innov_v = Xtot_v - odom_v;
test.innov_w = Xtot_w - odom_w;
test.innov_imu_a = Xtot_a - x_accel;
test.innov_imu_w = Xtot_w - imu_w;
//test.innov_v = kf_aux_.state_[0] - odom_v;
//test.innov_w = kf_aux_.state_[1] - odom_w;
//test.innov_imu_a = kf_enc_.state_[2] - x_accel;
//test.innov_imu_w = kf_enc_.state_[1] - imu_w;
test.innov_bound_v = 3*(sqrt(Ptot_v) + odom_var_v_);
test.innov_bound_w = 3*(sqrt(Ptot_w) + odom_var_w_);
test.innov_bound_imu_a = 3*(sqrt(Ptot_a) + imu_var_a_);
test.innov_bound_imu_w = 3*(sqrt(Ptot_w) + imu_var_w_);
test.gpsvelfix = gps_vel_fix;
test.gpsvel = gps_vel;
test.odomvel = odom_v;
// Calculate the certainty for each measurement:
// certainty = measurement / sqrt(covariance)
// if meas > 3*sqrt(cov), then there's like a 1% chance or less that
// the measurement is correct
double slip_enc_v, slip_enc_w, slip_gyro, slip_accel, slip_gps_v = 0;
if ( (sqrt(Ptot_v) + odom_var_v_) > .001)
slip_enc_v = fabs(test.innov_v) / (sqrt(Ptot_v) + odom_var_v_);
if ( (sqrt(Ptot_w) + odom_var_w_) > .001)
slip_enc_w = fabs(test.innov_w) / (sqrt(Ptot_w) + odom_var_w_);
if ( (sqrt(Ptot_w) + imu_var_w_) > .001)
slip_gyro = fabs(test.innov_imu_w) / (sqrt(Ptot_w) + imu_var_w_) ;
if ( (sqrt(Ptot_a) + imu_var_a_) > .001)
slip_accel = fabs(test.innov_imu_a) / (sqrt(Ptot_a) + imu_var_a_) ;
if ( (sqrt(Ptot_v) + gps_var_v_) > .001)
slip_gps_v = fabs(Xtot_v - gps_vel_fix) / (sqrt(Ptot_v) + gps_var_v_) ;
double max_innov = std::max(std::max(std::max(std::max(slip_enc_v, slip_gps_v),slip_enc_w),slip_gyro),slip_accel);
test.slip_enc_v = slip_enc_v/3;
test.slip_enc_w = slip_enc_w/3;
test.slip_gyro = slip_gyro/3;
test.slip_accel = slip_accel/3;
test.slip_gps_v = slip_gps_v/3;
test.slip_max = max_innov/3;
slip.slip_max = max_innov/3;
if (max_innov > 3) // Slip if the max innovation is > 3
slip.slip = 1;
else
slip.slip = 0;
// TODO: If we slip, maybe take the other filter out of the fused measurement??
// TODO: Implement an "integral term" on the slip.. If we're consistently have a larger
// than expected innovation, this could indicate a slip
// TODO: Have this node publish the filtered odometry??
slip_pub_.publish(slip);
imu_new_ = false;
odom_new_ = false;
gps_new_ = false;
}
if (no_update)
{
// Populate the kalman filter variables with the last state
test.corrected_imu_x = x_accel;
test.corrected_imu_y = y_accel;
test.kf_v = 0;
test.kf_w = 0;
test.kf_a = 0;
test.kf_wdot = 0;
}
test_pub_.publish(test);
};
void VideoCodecStatistics::Dump(RunningStat& s, const char *name)
{
CSFLogDebug(logTag,
"%s, mean: %f, variance: %f, standard deviation: %f",
name, s.Mean(), s.Variance(), s.StandardDeviation());
}
