feat(back): try to remove noise in geolocation tracking

back/src/Liane/Liane.Service/Internal/Util/KalmanFilter.cs

@@ -0,0 +1,44 @@
+using MathNet.Filtering.Kalman;
+using MathNet.Numerics.LinearAlgebra;
+
+namespace Liane.Service.Internal.Util;
+
+public sealed class KalmanFilter
+{
+  private readonly DiscreteKalmanFilter dkf;
+  private readonly Matrix<double> stateTransitionMatrixF;
+  private readonly Matrix<double> plantNoiseMatrixG;
+  private readonly Matrix<double> plantNoiseVarianceQ;
+  private readonly Matrix<double> measurementVarianceMatrixR;
+  private readonly Matrix<double> measurementMatrixH;
+
+  public KalmanFilter(double x0, double v0, double dt, double plantNoiseVar = 3, double measurementCovariance = 1.5)
+  {
+    var xState = Matrix<double>.Build.Dense(2, 1, [x0, v0]);
+
+    var measurementCovarianceMatrix = Matrix<double>.Build.Dense(2, 2,
+    [
+      measurementCovariance, measurementCovariance / dt,
+      measurementCovariance / dt, 2 * measurementCovariance / (dt * dt)
+    ]);
+
+    dkf = new DiscreteKalmanFilter(xState, measurementCovarianceMatrix);
+
+    stateTransitionMatrixF = Matrix<double>.Build.Dense(2, 2, [1d, 0d, dt, 1]);
+    plantNoiseMatrixG = Matrix<double>.Build.Dense(2, 1, [dt * dt / 2, dt]);
+    plantNoiseVarianceQ = plantNoiseMatrixG.Transpose() * plantNoiseMatrixG * plantNoiseVar;
+    measurementVarianceMatrixR = Matrix<double>.Build.Dense(1, 1, [measurementCovariance]);
+    measurementMatrixH = Matrix<double>.Build.Dense(1, 2, [1d, 0d]);
+  }
+
+  public void Update(double x, double y, double dt)
+  {
+    var z = Matrix<double>.Build.Dense(1, 2, [x, y]);
+    stateTransitionMatrixF[0, 1] = dt;
+    plantNoiseMatrixG[0, 0] = dt * dt / 2;
+    plantNoiseMatrixG[1, 0] = dt;
+
+    dkf.Predict(stateTransitionMatrixF, plantNoiseMatrixG, plantNoiseVarianceQ);
+    dkf.Update(z, measurementMatrixH, measurementVarianceMatrixR);
+  }
+}

back/src/Liane/Liane.Service/Liane.Service.csproj

@@ -17,9 +17,10 @@
         <PackageReference Include="dbup-postgresql" Version="5.0.40" />
         <PackageReference Include="FirebaseAdmin" Version="2.4.0" />
         <PackageReference Include="GeoJSON.Text" Version="1.0.2" />
+        <PackageReference Include="MathNet.Filtering.Kalman" Version="0.7.0" />
         <PackageReference Include="MongoDB.Driver" Version="2.23.1" />
-        <PackageReference Include="Npgsql" Version="8.0.2" />
-        <PackageReference Include="Npgsql.GeoJSON" Version="8.0.2" />
+        <PackageReference Include="Npgsql" Version="8.0.3" />
+        <PackageReference Include="Npgsql.GeoJSON" Version="8.0.3" />
         <PackageReference Include="Twilio" Version="6.16.1" />
         <PackageReference Include="NodaTime" Version="3.1.11" />
         <PackageReference Include="UuidExtensions" Version="1.2.0" />

back/src/Liane/Liane.Test/Integration/LianeTrackerTest.cs

@@ -1,57 +1,33 @@
 using System;
 using System.Collections.Generic;
 using System.Collections.Immutable;
+using System.Globalization;
 using System.Linq;
 using System.Text.Json;
 using System.Threading.Tasks;
 using GeoJSON.Text.Feature;
-using GeoJSON.Text.Geometry;
+using Liane.Api.Auth;
 using Liane.Api.Routing;
 using Liane.Api.Trip;
-using Liane.Api.Auth;
 using Liane.Api.Util.Ref;
 using Liane.Service.Internal.Mongo;
 using Liane.Service.Internal.Trip;
 using Liane.Service.Internal.Trip.Geolocation;
+using Liane.Service.Internal.Util;
 using Liane.Test.Util;
 using Microsoft.Extensions.DependencyInjection;
 using MongoDB.Bson;
 using MongoDB.Bson.Serialization;
 using MongoDB.Driver;
+using MongoDB.Driver.GeoJsonObjectModel;
 using NUnit.Framework;
+using Point = GeoJSON.Text.Geometry.Point;
 
 namespace Liane.Test.Integration;
 
 [TestFixture(Category = "Integration")]
 public sealed class LianeTrackerTest : BaseIntegrationTest
 {
-  private static (Api.Trip.Trip liane, FeatureCollection pings) PrepareTestData(Ref<User> userId)
-  {
-    var departureTime = DateTime.Parse("2023-08-08T16:12:53.061Z");
-    var liane = new Api.Trip.Trip(
-      "6410edc1e02078e7108a5895",
-      userId,
-      DateTime.Today,
-      departureTime,
-      null,
-      new List<WayPoint>
-      {
-        new(LabeledPositions.Tournefeuille, 0, 0, departureTime),
-        new(LabeledPositions.AireDesPyrénées, 45 * 60, 65000, DateTime.Parse("2023-08-08T16:57:54.000Z")),
-        new(LabeledPositions.PointisInard, 19 * 60, 24000, DateTime.Parse("2023-08-08T17:17:05.000Z"))
-      }.ToImmutableList(),
-      new List<LianeMember>
-      {
-        new(userId, LabeledPositions.Tournefeuille, LabeledPositions.PointisInard, 3),
-        new("63f7a5c90f65806b1adb3081", LabeledPositions.Tournefeuille, LabeledPositions.AireDesPyrénées)
-      }.ToImmutableList(),
-      new Driver(userId),
-      LianeState.NotStarted,
-      null
-    );
-    return (liane, JsonSerializer.Deserialize<FeatureCollection>(AssertExtensions.ReadTestResource("Geolocation/test-tournefeuille-pointis-inard.json"))!);
-  }
-
   [Test]
   public async Task ShouldFinishTrip()
   {
@@ -66,7 +42,7 @@ public async Task ShouldFinishTrip()
       var point = (f.Geometry as Point)!;
       var time = f.Properties["timestamp"].ToString()!;
       var user = f.Properties["user"].ToString()!;
-      return (timestamp: DateTime.Parse(time), coordinate: new LatLng(point.Coordinates.Latitude, point.Coordinates.Longitude), user: user);
+      return (timestamp: DateTime.Parse(time), coordinate: new LatLng(point.Coordinates.Latitude, point.Coordinates.Longitude), user);
     });
     foreach (var p in pings.OrderBy(p => p.timestamp))
     {
@@ -141,8 +117,8 @@ public async Task ShouldNotFinishTrip()
     var bson = BsonDocument.Parse(AssertExtensions.ReadTestResource("Geolocation/liane-pings-case-1.json"));
     var lianeDb = BsonSerializer.Deserialize<LianeDb>(bson);
     var userIds = lianeDb.Members.Select((m, i) => (m, i)).ToDictionary(m => m.m.User, m => Fakers.FakeDbUsers[m.i].Id);
-    lianeDb = lianeDb with { Members = lianeDb.Members.Select((m, i) => m with { User = userIds[m.User] }).ToImmutableList() };
-    await Db.GetCollection<LianeDb>().InsertOneAsync(lianeDb);
+    lianeDb = lianeDb with { Members = lianeDb.Members.Select((m, _) => m with { User = userIds[m.User] }).ToImmutableList() };
+    await db.GetCollection<LianeDb>().InsertOneAsync(lianeDb);
     var liane = await tripService.Get(lianeDb.Id);
     var tracker = await lianeTrackerService.Start(liane, () => { finished = true; });
     var pings = lianeDb.Pings.OrderBy(p => p.At).Select(p => p with { User = userIds[p.User] }).ToImmutableList();
@@ -166,7 +142,7 @@ public async Task ShouldTrackNextWayPointWithPassengers()
 
     // check that next point is Quezac (the car is going towards Quezac)
     var actual = tracker.GetTrackingInfo();
-    Assert.AreEqual("Quezac_Parking", actual.Car.NextPoint.Id);
+    Assert.AreEqual("Quezac_Parking", actual.Car?.NextPoint.Id);
   }
 
   [Test]
@@ -177,8 +153,8 @@ public async Task ShouldTrackFirstWayPointWithPassengers()
 
     // check that next point is Quezac (the car is going towards Quezac)
     var actual = tracker.GetTrackingInfo();
-    Assert.AreEqual("Quezac_Parking", actual.Car.NextPoint.Id);
-    Assert.Less(Math.Abs(actual.Car.Delay - 562), 10); // Check difference with expected value is less than 10 seconds
+    Assert.AreEqual("Quezac_Parking", actual.Car?.NextPoint.Id);
+    Assert.Less(Math.Abs(actual.Car!.Delay - 562), 10); // Check difference with expected value is less than 10 seconds
     Assert.AreEqual(DateTime.Parse("2023-12-05T07:37:34.113Z").ToUniversalTime(), actual.Car.At);
   }
 
@@ -191,8 +167,8 @@ public async Task ShouldTrackDestinationWayPoint()
     var actual = tracker.GetTrackingInfo();
 
     // check that next point is Mende 
-    Assert.AreEqual("Mende", actual.Car.NextPoint.Id);
-    Assert.Less(Math.Abs(actual.Car.Delay - 1450), 10); // Check difference with expected value is less than 10 seconds
+    Assert.AreEqual("Mende", actual.Car?.NextPoint.Id);
+    Assert.Less(Math.Abs(actual.Car!.Delay - 1450), 10); // Check difference with expected value is less than 10 seconds
   }
 
   [Test]
@@ -202,7 +178,7 @@ public async Task ShouldNotHavePassengersInCarWhenNoPassengersPing()
 
     var actual = tracker.GetTrackingInfo();
     // Check who's in the car
-    CollectionAssert.AreEquivalent(ImmutableList.Create(tracker.Trip.Driver.User.Id), actual.Car.Members.Select(m => m.Id));
+    CollectionAssert.AreEquivalent(ImmutableList.Create(tracker.Trip.Driver.User.Id), actual.Car?.Members.Select(m => m.Id));
   }
 
   [Test]
@@ -212,7 +188,7 @@ public async Task ShouldNotHavePassengersInCarBeforePickup()
 
     var actual = tracker.GetTrackingInfo();
     // Check who's in the car
-    CollectionAssert.AreEquivalent(ImmutableList.Create(tracker.Trip.Driver.User.Id), actual.Car.Members.Select(m => m.Id));
+    CollectionAssert.AreEquivalent(ImmutableList.Create(tracker.Trip.Driver.User.Id), actual.Car?.Members.Select(m => m.Id));
     // Check we do have location for the other member
     Assert.AreEqual(1, actual.OtherMembers.Count);
     Assert.AreNotEqual(tracker.Trip.Driver.User.Id, actual.OtherMembers.Keys.First());
@@ -225,15 +201,15 @@ public async Task ShouldHavePassengersInCar()
 
     var actual = tracker.GetTrackingInfo();
     // Check who's in the car
-    CollectionAssert.AreEquivalent(tracker.Trip.Members.Select(m => m.User.Id), actual.Car.Members.Select(m => m.Id));
+    CollectionAssert.AreEquivalent(tracker.Trip.Members.Select(m => m.User.Id), actual.Car?.Members.Select(m => m.Id));
     // Check car's current location
     var expectedLocation = tracker.GetCurrentMemberLocation(
       tracker.Trip.Members.First(m => m.User != tracker.Trip.Driver.User
       ).User)?.Location;
-    Assert.Less(1, actual.Car.Position.Distance(expectedLocation!.Value));
+    Assert.Less(1, actual.Car?.Position.Distance(expectedLocation!.Value));
   }
 
-  private async Task<(LianeTracker, ImmutableList<UserPing>, ImmutableDictionary<Ref<User>, Ref<User>>)> SetupTracker(string file)
+  private async Task<(LianeTracker, ImmutableList<UserPing>, ImmutableDictionary<Ref<User>, Ref<User>>, Api.Trip.Trip)> SetupTracker(string file)
   {
     var bson = BsonDocument.Parse(AssertExtensions.ReadTestResource(file));
     var lianeDb = BsonSerializer.Deserialize<LianeDb>(bson);
@@ -244,27 +220,68 @@ public async Task ShouldHavePassengersInCar()
       Members = lianeDb.Members.Select(m => m with { User = userMapping[m.User] }).ToImmutableList(),
       Pings = lianeDb.Pings.Select(p => p with { User = userMapping[p.User] }).ToImmutableList()
     };
-    await Db.GetCollection<LianeDb>().InsertOneAsync(lianeDb);
+    await db.GetCollection<LianeDb>().InsertOneAsync(lianeDb);
     var liane = await tripService.Get(lianeDb.Id);
     var tracker = await lianeTrackerService.Start(liane);
     var pings = lianeDb.Pings
       .OrderBy(p => p.At)
       .ToImmutableList();
 
-    return (tracker, pings, userMapping.ToImmutableDictionary(e => e.Value, e => e.Key));
+    return (tracker, pings, userMapping.ToImmutableDictionary(e => e.Value, e => e.Key), liane);
   }
 
-  private async Task<(LianeTracker, ImmutableDictionary<Ref<User>, Ref<User>>)> SetupTrackerAt(string file, string? at = null)
+  private async Task<(LianeTracker, ImmutableDictionary<Ref<User>, Ref<User>>)> SetupTrackerAt(string file, string? at = null, bool debugGeoJson = false)
   {
-    var (tracker, pings, userMapping) = await SetupTracker(file);
+    var (tracker, pings, userMapping, trip) = await SetupTracker(file);
     // Send first few pings outside of planned route
     var sublist = (at is null ? pings : pings.TakeWhile(p => p.At.ToUniversalTime() < DateTime.Parse(at).ToUniversalTime())).ToList();
 
+    var startAt = sublist.First().At;
+    var kalmanFilter = new KalmanFilter(0, 0, 0);
     foreach (var p in sublist)
     {
+      if (p.Coordinate is null)
+      {
+        continue;
+      }
+
+      kalmanFilter.Update(p.Coordinate.Value.Lng, p.Coordinate.Value.Lat, (p.At - startAt).TotalMilliseconds);
+    }
+
+    var index = 0;
+    var geoJson = new List<GeoJsonFeature<GeoJson2DGeographicCoordinates>>();
+    foreach (var p in sublist)
+    {
+      var args = new GeoJsonFeatureArgs<GeoJson2DGeographicCoordinates>();
+      var properties = new BsonDocument
+      {
+        ["user"] = p.User.ToString(),
+        ["timestamp"] = p.At.ToString(CultureInfo.GetCultureInfo("FR_fr")),
+        ["index"] = index++
+      };
+
+      args.ExtraMembers = new BsonDocument
+      {
+        ["properties"] = properties
+      };
+
+      geoJson.Add(new GeoJsonFeature<GeoJson2DGeographicCoordinates>(args, new GeoJsonPoint<GeoJson2DGeographicCoordinates>(
+        new GeoJson2DGeographicCoordinates(p.Coordinate.Value.Lng, p.Coordinate.Value.Lat)
+      )));
       await lianeTrackerService.PushPing(tracker.Trip, p);
     }
 
+    if (debugGeoJson)
+    {
+      var routingService = ServiceProvider.GetRequiredService<IRoutingService>();
+      var simplifiedRoute =
+        new GeoJsonFeature<GeoJson2DGeographicCoordinates>((await routingService.GetRoute(trip.WayPoints.Select(w => w.RallyingPoint.Location).ToImmutableList())).Coordinates.ToLatLng().ToGeoJson());
+      geoJson.Add(simplifiedRoute);
+
+      var collection = new GeoJsonFeatureCollection<GeoJson2DGeographicCoordinates>(geoJson);
+      Console.WriteLine("GEOJSON : {0}", collection.ToJson());
+    }
+
     return (tracker, userMapping);
   }
 
@@ -327,7 +344,7 @@ public async Task Trip_qui_se_trouve_deja_a_destination()
     // Départ : Roques
     // Arrivée : LivingObjects
 
-    // driver thibauls : 6617e60b606952ceee7ee2aa
+    // driver thibault : 6617e60b606952ceee7ee2aa
     // augustin : ba3
 
     // thibault se trouve déjà à destination
@@ -338,15 +355,62 @@ public async Task Trip_qui_se_trouve_deja_a_destination()
     Assert.AreEqual("custom:001", actual.Car?.NextPoint.Id);
   }
 
+  [Test]
+  public async Task Trip_with_noise()
+  {
+    var (tracker, userMapping) = await SetupTrackerAt("Geolocation/Trip_with_noise.json", debugGeoJson: true);
+
+    // Départ : Roques
+    // Arrivée : LivingObjects
+
+    // driver thibault : 6617e60b606952ceee7ee2aa
+    // augustin : ba3
+
+    // thibault se trouve déjà à destination
+    //await lianeTrackerService.PushPing("6617e60b606952ceee7ee2aa", new UserPing("65f2cbd9e94a0516ac1e6dac", DateTime.Parse("2024-04-11T13:03:00+1"), TimeSpan.Zero, new LatLng(3.4845875, 44.3378072)));
+
+    var actual = tracker.GetTrackingInfo();
+
+    Assert.AreEqual("bnlc:07127-C-001", actual.Car?.NextPoint.Id);
+  }
+
+  private IMongoDatabase db = null!;
 
-  private IMongoDatabase Db = null!;
   private ITripService tripService = null!;
+
   private ILianeTrackerService lianeTrackerService = null!;
 
-  protected override void Setup(IMongoDatabase db)
+  protected override void Setup(IMongoDatabase database)
   {
-    Db = db;
+    db = database;
     tripService = ServiceProvider.GetRequiredService<ITripService>();
     lianeTrackerService = ServiceProvider.GetRequiredService<ILianeTrackerService>();
   }
+
+  private static (Api.Trip.Trip liane, FeatureCollection pings) PrepareTestData(Ref<User> userId)
+  {
+    var departureTime = DateTime.Parse("2023-08-08T16:12:53.061Z");
+    var liane = new Api.Trip.Trip(
+      "6410edc1e02078e7108a5895",
+      userId,
+      DateTime.Today,
+      departureTime,
+      null,
+      new List<WayPoint>
+      {
+        new(LabeledPositions.Tournefeuille, 0, 0, departureTime),
+        new(LabeledPositions.AireDesPyrénées, 45 * 60, 65000, DateTime.Parse("2023-08-08T16:57:54.000Z")),
+        new(LabeledPositions.PointisInard, 19 * 60, 24000, DateTime.Parse("2023-08-08T17:17:05.000Z"))
+      }.ToImmutableList(),
+      new List<LianeMember>
+      {
+        new(userId, LabeledPositions.Tournefeuille, LabeledPositions.PointisInard, 3),
+        new("63f7a5c90f65806b1adb3081", LabeledPositions.Tournefeuille, LabeledPositions.AireDesPyrénées)
+      }.ToImmutableList(),
+      new Driver(userId),
+      LianeState.NotStarted,
+      null
+    );
+    return (liane, JsonSerializer.Deserialize<FeatureCollection>(AssertExtensions.ReadTestResource("Geolocation/test-tournefeuille-pointis-inard.json"))!);
+  }
 }

back/src/Liane/Liane.Test/LabeledPositions.cs

@@ -93,6 +93,8 @@ public sealed class LabeledPositions
     "Balsièges", null, true);
 
   public static readonly RallyingPoint LO = new("custom:001", "Living Objects", Positions.LO, LocationType.Parking, "1 impasse Marcel Chalard", "31000", "Toulouse", 10, true);
+  public static readonly RallyingPoint ArdecheBeage = new("mairie:07026", "Le Beage", Positions.ArdecheLeBeage, LocationType.TownHall, "Le Village", "07630", "Le Béage", 10, true);
+  public static readonly RallyingPoint ArdecheBoulodrome = new("bnlc:07127-C-001", "Boulodrome", Positions.ArdecheBoulodrome, LocationType.Parking, "33 Avenue Centrale. 07380 Lalevade-d'Ardéche", "07380", "Lalevade-d'Ardéche", 10, true);
 
   public static readonly IImmutableSet<RallyingPoint> RallyingPoints = ImmutableHashSet.Create(
     Mende,
@@ -133,6 +135,8 @@ public sealed class LabeledPositions
     MartresTolosane,
     LO,
     Montbrun,
-    Choizal
+    Choizal,
+    ArdecheBeage,
+    ArdecheBoulodrome
   );
 }

back/src/Liane/Liane.Test/Positions.cs

@@ -15,7 +15,7 @@ public sealed class Positions
   public static readonly LatLng GorgesDuTarnCausses = new(44.36512686514503, 3.414840996265411);
   public static readonly LatLng SaintEnimieParking = new(44.36545637889574, 3.412078320980072);
   public static readonly LatLng Florac = new(44.319013, 3.578021);
-  public static readonly LatLng FloracFormares = new(44.335825,3.5847193);
+  public static readonly LatLng FloracFormares = new(44.335825, 3.5847193);
   public static readonly LatLng SaintChelyDuTarnEnHaut = new(44.338294306952264, 3.381930291652679);
   public static readonly LatLng ChamperbouxEglise = new(44.40842497198885, 3.414583504199982);
   public static readonly LatLng LavalDuTarnEglise = new(44.3533150645425, 3.352039754390717);
@@ -44,4 +44,6 @@ public sealed class Positions
   public static readonly LatLng Choizal = new(44.4595512, 3.452795);
   public static readonly LatLng LO = new(43.567936, 1.390924);
   public static readonly LatLng Lavogne = new(44.414532, 3.5101363);
+  public static readonly LatLng ArdecheLeBeage = new(44.849155426, 4.11706209183);
+  public static readonly LatLng ArdecheBoulodrome = new(44.651711, 4.322269);
 }