Exemple #1
0
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);

};
Exemple #3
0
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());
}