<commit_before>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude = ( S.Latitude*Measurement.Latitude.Variance + Measurement.Latitude*S.Latitude.Variance ) / (S.Latitude.Variance+Measurement.Latitude.Variance);
    result.Longitude = ( S.Longitude*Measurement.Longitude.Variance + Measurement.Longitude*S.Longitude.Variance ) / (S.Longitude.Variance+Measurement.Longitude.Variance);
    result.Heading = ( S.Heading*Measurement.Heading.Variance + Measurement.Heading*S.Heading.Variance ) / (S.Heading.Variance+Measurement.Heading.Variance);

    result.Latitude.Variance = ( 1.0 / (1.0/S.Latitude.Variance + 1.0 / Measurement.Latitude.Variance) );
    result.Longitude.Variance = ( 1.0 / (1.0/S.Longitude.Variance + 1.0 / Measurement.Longitude.Variance) );
    result.Heading.Variance = ( 1.0 / (1.0/S.Heading.Variance + 1.0 / Measurement.Heading.Variance) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude = S.Latitude + Delta.Latitude;
    result.Longitude = S.Longitude + Delta.Longitude;
    result.Heading = S.Heading + Delta.Heading;

    result.Latitude.Variance = S.Latitude.Variance + Delta.Latitude.Variance;
    result.Longitude.Variance = S.Longitude.Variance + Delta.Longitude.Variance;
    result.Heading.Variance = S.Heading.Variance + Delta.Heading.Variance;
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::Instance().getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading + W*t) * M_PI/180.0;
    delta.Heading = W*t;
    delta.Latitude = R*(cos(theta) - cos(thetaPrime));
    delta.Longitude = R*(sin(thetaPrime) - sin(theta));
    // TODO - handle variances
    return delta;
}

Position PositionTracker::MeasurementFromIMUData(IMUData data)
{
    // Uses the "Destination point given distance and bearing from start point" described here:
    // http://www.movable-type.co.uk/scripts/latlong.html#destPoint
    Position measurement;
    double lat1 = _current_estimate.Latitude;
    double lon1 = _current_estimate.Latitude;
    double hed1 = _current_estimate.Heading;
    double dx = data.X /* * time*time */;
    double dy = data.Y /* * time*time */;
    double d = sqrt(dx*dx + dy*dy); // Distance travelled
    double R = 6378137; // radius of Earth
    measurement.Latitude = asin(sin(lat1)*cos(d/R)+cos(lat1)*sin(d/R)*cos(hed1));
    measurement.Longitude = (lon1 + atan2(sin(hed1)*sin(d/R)*cos(lat1),cos(d/R)-sin(lat1)*sin(measurement.Latitude)));
    measurement.Heading = (atan2((measurement.Longitude-lon1)*sin(lat1), cos(measurement.Latitude)*sin(lat1)-sin(measurement.Latitude)*cos(lat1)*cos(measurement.Longitude-lon1)));
    measurement.Heading = (measurement.Heading+180);
    while(measurement.Heading >= 360)
        measurement.Heading = (measurement.Heading - 360);
    while(measurement.Heading < 0)
        measurement.Heading = (measurement.Heading + 360);
    // TODO - handle variances
    return measurement;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude = data.Lat();
    measurement.Longitude = data.Long();
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading = data.Heading();
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{
    _current_estimate = UpdateWithMeasurement(_current_estimate, MeasurementFromIMUData(data));
}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}
<commit_msg>Adds some TODO comments to PositionTracker.<commit_after>#include "positiontracker.h"
#include <cmath>
#include <common/config/configmanager.h>

PositionTracker::PositionTracker()
    : LOnGPSData(this),
      LOnMotionCommand(this),
      LOnIMUData(this)
{
}

Position PositionTracker::GetPosition()
{
    return _current_estimate;
}

Position PositionTracker::UpdateWithMeasurement(Position S, Position Measurement)
{
    Position result;
    result.Latitude = ( S.Latitude*Measurement.Latitude.Variance + Measurement.Latitude*S.Latitude.Variance ) / (S.Latitude.Variance+Measurement.Latitude.Variance);
    result.Longitude = ( S.Longitude*Measurement.Longitude.Variance + Measurement.Longitude*S.Longitude.Variance ) / (S.Longitude.Variance+Measurement.Longitude.Variance);
    result.Heading = ( S.Heading*Measurement.Heading.Variance + Measurement.Heading*S.Heading.Variance ) / (S.Heading.Variance+Measurement.Heading.Variance);

    result.Latitude.Variance = ( 1.0 / (1.0/S.Latitude.Variance + 1.0 / Measurement.Latitude.Variance) );
    result.Longitude.Variance = ( 1.0 / (1.0/S.Longitude.Variance + 1.0 / Measurement.Longitude.Variance) );
    result.Heading.Variance = ( 1.0 / (1.0/S.Heading.Variance + 1.0 / Measurement.Heading.Variance) );
    return result;
}

Position PositionTracker::UpdateWithMotion(Position S, Position Delta)
{
    Position result;
    result.Latitude = S.Latitude + Delta.Latitude;
    result.Longitude = S.Longitude + Delta.Longitude;
    result.Heading = S.Heading + Delta.Heading;

    result.Latitude.Variance = S.Latitude.Variance + Delta.Latitude.Variance;
    result.Longitude.Variance = S.Longitude.Variance + Delta.Longitude.Variance;
    result.Heading.Variance = S.Heading.Variance + Delta.Heading.Variance;
    return result;
}

Position PositionTracker::DeltaFromMotionCommand(MotorCommand cmd)
{
    /*
     * TODO - Need to make sure this accounts for the time it takes to actually get to where we're going.
     * Perhaps fire this after the motion command is done.
     */
    using namespace std;
    double V = ( cmd.rightVel + cmd.leftVel ) / 2.0;
    double W = ( cmd.rightVel - cmd.leftVel ) / ConfigManager::Instance().getValue("Robot", "Baseline", 1.0);
    double t = cmd.millis / 1000.0;
    double R = V/W;
    Position delta;
    double theta = _current_estimate.Heading * M_PI/180.0;
    double thetaPrime = (_current_estimate.Heading + W*t) * M_PI/180.0;
    delta.Heading = W*t;
    delta.Latitude = R*(cos(theta) - cos(thetaPrime));
    delta.Longitude = R*(sin(thetaPrime) - sin(theta));
    // TODO - handle variances
    return delta;
}

Position PositionTracker::MeasurementFromIMUData(IMUData data)
{
    // Uses the "Destination point given distance and bearing from start point" described here:
    // http://www.movable-type.co.uk/scripts/latlong.html#destPoint
    Position measurement;
    double lat1 = _current_estimate.Latitude;
    double lon1 = _current_estimate.Latitude;
    double hed1 = _current_estimate.Heading;
    double dx = data.X /* * time*time */;
    double dy = data.Y /* * time*time */;
    double d = sqrt(dx*dx + dy*dy); // Distance travelled
    double R = 6378137; // radius of Earth
    measurement.Latitude = asin(sin(lat1)*cos(d/R)+cos(lat1)*sin(d/R)*cos(hed1));
    measurement.Longitude = (lon1 + atan2(sin(hed1)*sin(d/R)*cos(lat1),cos(d/R)-sin(lat1)*sin(measurement.Latitude)));
    measurement.Heading = (atan2((measurement.Longitude-lon1)*sin(lat1), cos(measurement.Latitude)*sin(lat1)-sin(measurement.Latitude)*cos(lat1)*cos(measurement.Longitude-lon1)));
    measurement.Heading = (measurement.Heading+180);
    while(measurement.Heading >= 360)
        measurement.Heading = (measurement.Heading - 360);
    while(measurement.Heading < 0)
        measurement.Heading = (measurement.Heading + 360);
    // TODO - handle variances
    return measurement;
}

void PositionTracker::OnGPSData(GPSData data)
{
    Position measurement;
    measurement.Latitude = data.Lat();
    measurement.Longitude = data.Long();
    // TODO - make sure the GPS actually outputs heading data
    measurement.Heading = data.Heading();
    // TODO - handle variances
    _current_estimate = UpdateWithMeasurement(_current_estimate, measurement);
}

void PositionTracker::OnIMUData(IMUData data)
{
    _current_estimate = UpdateWithMeasurement(_current_estimate, MeasurementFromIMUData(data));
}

void PositionTracker::OnMotionCommand(MotorCommand cmd)
{
    /*
     * TODO - Need to make sure this accounts for the time it takes to actually get to where we're going.
     * Perhaps fire this after the motion command is done.
     */
    _current_estimate = UpdateWithMotion(_current_estimate, DeltaFromMotionCommand(cmd));
}

{'fraction_non_alphanumeric': 0.12602164789043516, 'fraction_numerical': 0.015904572564612324, 'mean_word_length': 4.055834729201563, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 9, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5101225', 'n_tokens_mistral': 3032, 'n_tokens_neox': 2907, 'n_words': 535}