From db9b254d7358ad376524bddb9da9e898b603742d Mon Sep 17 00:00:00 2001 From: Darrell Date: Sun, 1 Sep 2024 18:10:56 -0400 Subject: [PATCH] Add Kalman Filter --- src/Locators/BfgsMultilateralizer.cs | 6 +- src/Locators/GaussNewtonMultilateralizer.cs | 8 +- .../IterativeCentroidMultilateralizer.cs | 4 +- src/Locators/MLEMultilateralizer.cs | 8 +- src/Locators/MultiFloorMultilateralizer.cs | 6 +- src/Locators/NelderMeadMultilateralizer.cs | 8 +- src/Models/Scenario.cs | 80 ++++++++++++++++++- 7 files changed, 98 insertions(+), 22 deletions(-) diff --git a/src/Locators/BfgsMultilateralizer.cs b/src/Locators/BfgsMultilateralizer.cs index f5dd23bf..6a561a78 100644 --- a/src/Locators/BfgsMultilateralizer.cs +++ b/src/Locators/BfgsMultilateralizer.cs @@ -49,7 +49,7 @@ public bool Locate(Scenario scenario) if (pos.Length < 3 || _floor.Bounds == null) { confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } else { @@ -77,7 +77,7 @@ public bool Locate(Scenario scenario) }); var solver = new BfgsBMinimizer(0, 0.25, 0.25, 1000); var result = solver.FindMinimum(obj, lowerBound, upperBound, initialGuess); - scenario.Location = new Point3D(result.MinimizingPoint[0], result.MinimizingPoint[1], result.MinimizingPoint[2]); + scenario.UpdateLocation(new Point3D(result.MinimizingPoint[0], result.MinimizingPoint[1], result.MinimizingPoint[2])); scenario.Fixes = pos.Length; scenario.Error = result.FunctionInfoAtMinimum.Value; scenario.Iterations = result switch @@ -94,7 +94,7 @@ public bool Locate(Scenario scenario) catch (Exception ex) { confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); Log.Error("Error finding location for {0}: {1}", _device, ex.Message); } diff --git a/src/Locators/GaussNewtonMultilateralizer.cs b/src/Locators/GaussNewtonMultilateralizer.cs index 447f1287..6f35f87e 100644 --- a/src/Locators/GaussNewtonMultilateralizer.cs +++ b/src/Locators/GaussNewtonMultilateralizer.cs @@ -53,7 +53,7 @@ public bool Locate(Scenario scenario) if (pos.Length < 3 || _floor.Bounds == null) { confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } else { @@ -67,7 +67,7 @@ public bool Locate(Scenario scenario) var gaussNewton = new GaussNewton(pos, ranges, lowerBound, upperBound); var result = gaussNewton.FindPosition(initialGuess); - scenario.Location = new Point3D(result.X, result.Y, result.Z); + scenario.UpdateLocation(new Point3D(result.X, result.Y, result.Z)); scenario.Fixes = pos.Length; scenario.Error = pos.Select((p, i) => Math.Pow(Error(result.ToPoint3D(), p.ToPoint3D(), ranges[i]), 2)).Average(); scenario.Iterations = gaussNewton.Iterations; @@ -81,12 +81,12 @@ public bool Locate(Scenario scenario) { scenario.ReasonForExit = ExitCondition.ExceedIterations; confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } catch (Exception ex) { confidence = 0; - scenario.Location = new Point3D(); + scenario.UpdateLocation(new Point3D()); Log.Error("Error finding location for {0}: {1}", _device, ex.Message); } diff --git a/src/Locators/IterativeCentroidMultilateralizer.cs b/src/Locators/IterativeCentroidMultilateralizer.cs index b30c1c40..635708c9 100644 --- a/src/Locators/IterativeCentroidMultilateralizer.cs +++ b/src/Locators/IterativeCentroidMultilateralizer.cs @@ -49,7 +49,7 @@ public bool Locate(Scenario scenario) scenario.Error = CalculateError(centroid, original); scenario.Confidence = 1; scenario.ReasonForExit = ExitCondition.LackOfProgress; - scenario.Location = new Point3D(centroid[0], centroid[1], centroid[2]); + scenario.UpdateLocation(new Point3D(centroid[0], centroid[1], centroid[2])); scenario.Room = floor.Rooms.Values.FirstOrDefault(a => a.Polygon?.EnclosesPoint(scenario.Location.ToPoint2D()) ?? false); return Math.Abs(scenario.Location.DistanceTo(scenario.LastLocation)) >= 0.1; } @@ -65,7 +65,7 @@ public bool Locate(Scenario scenario) var err = CalculateError(centroid, original); scenario.Error = err; scenario.Confidence = Math.Clamp((10000 - (int?)Math.Ceiling(100 * err)) ?? 0, 0, 100); - scenario.Location = new Point3D(centroid[0], centroid[1], centroid[2]); + scenario.UpdateLocation(new Point3D(centroid[0], centroid[1], centroid[2])); scenario.Room = floor.Rooms.Values.FirstOrDefault(a => a.Polygon?.EnclosesPoint(scenario.Location.ToPoint2D()) ?? false); return Math.Abs(scenario.Location.DistanceTo(scenario.LastLocation)) >= 0.1; } diff --git a/src/Locators/MLEMultilateralizer.cs b/src/Locators/MLEMultilateralizer.cs index 11d413ee..a486650a 100644 --- a/src/Locators/MLEMultilateralizer.cs +++ b/src/Locators/MLEMultilateralizer.cs @@ -52,7 +52,7 @@ double Error(IList x, DeviceToNode dn) if (pos.Length < 3 || floor.Bounds == null) { confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } else { @@ -84,7 +84,7 @@ double Error(IList x, DeviceToNode dn) var solver = new NelderMeadSimplex(1e-7, 10000); var result = solver.FindMinimum(obj, initialGuess, initialPerturbation); var minimizingPoint = result.MinimizingPoint.PointwiseMinimum(upperBound).PointwiseMaximum(lowerBound); - scenario.Location = new Point3D(minimizingPoint[0], minimizingPoint[1], minimizingPoint[2]); + scenario.UpdateLocation(new Point3D(minimizingPoint[0], minimizingPoint[1], minimizingPoint[2])); scenario.Scale = minimizingPoint[3]; scenario.Fixes = pos.Length; scenario.Error = result.FunctionInfoAtMinimum.Value; @@ -103,12 +103,12 @@ double Error(IList x, DeviceToNode dn) { scenario.ReasonForExit = ExitCondition.ExceedIterations; confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } catch (Exception ex) { confidence = 0; - scenario.Location = new Point3D(); + scenario.UpdateLocation(new Point3D()); Log.Error("Error finding location for {0}: {1}", device, ex.Message); } diff --git a/src/Locators/MultiFloorMultilateralizer.cs b/src/Locators/MultiFloorMultilateralizer.cs index b243075e..babcc04a 100644 --- a/src/Locators/MultiFloorMultilateralizer.cs +++ b/src/Locators/MultiFloorMultilateralizer.cs @@ -61,7 +61,7 @@ public bool Locate(Scenario scenario) scenario.Scale ?? 1.0 }); var result = solver.FindMinimum(obj, init); - scenario.Location = new Point3D(result.MinimizingPoint[0], result.MinimizingPoint[1], result.MinimizingPoint[2]); + scenario.UpdateLocation(new Point3D(result.MinimizingPoint[0], result.MinimizingPoint[1], result.MinimizingPoint[2])); scenario.Scale = result.MinimizingPoint[3]; scenario.Fixes = pos.Count; scenario.Minimum = nodes.Min(a => (double?) a.Distance); @@ -73,14 +73,14 @@ public bool Locate(Scenario scenario) { confidence = 1; scenario.Scale = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } } catch (Exception ex) { confidence = 1; scenario.Scale = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); Log.Error("Error finding location for {0}: {1}", _device, ex.Message); } } diff --git a/src/Locators/NelderMeadMultilateralizer.cs b/src/Locators/NelderMeadMultilateralizer.cs index 8a4ed458..e29ff03c 100644 --- a/src/Locators/NelderMeadMultilateralizer.cs +++ b/src/Locators/NelderMeadMultilateralizer.cs @@ -41,7 +41,7 @@ public bool Locate(Scenario scenario) if (pos.Length < 3 || floor.Bounds == null) { confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } else { @@ -73,7 +73,7 @@ public bool Locate(Scenario scenario) var solver = new NelderMeadSimplex(1e-7, 10000); var result = solver.FindMinimum(obj, initialGuess, initialPerturbation); var minimizingPoint = result.MinimizingPoint.PointwiseMinimum(upperBound).PointwiseMaximum(lowerBound); - scenario.Location = new Point3D(minimizingPoint[0], minimizingPoint[1], minimizingPoint[2]); + scenario.UpdateLocation(new Point3D(minimizingPoint[0], minimizingPoint[1], minimizingPoint[2])); scenario.Scale = minimizingPoint[3]; scenario.Fixes = pos.Length; scenario.Error = result.FunctionInfoAtMinimum.Value; @@ -92,12 +92,12 @@ public bool Locate(Scenario scenario) { scenario.ReasonForExit = ExitCondition.ExceedIterations; confidence = 1; - scenario.Location = guess; + scenario.UpdateLocation(guess); } catch (Exception ex) { confidence = 0; - scenario.Location = new Point3D(); + scenario.UpdateLocation(new Point3D()); Log.Error("Error finding location for {0}: {1}", device, ex.Message); } diff --git a/src/Models/Scenario.cs b/src/Models/Scenario.cs index e3d0d2f2..c88d6fb5 100644 --- a/src/Models/Scenario.cs +++ b/src/Models/Scenario.cs @@ -2,6 +2,7 @@ using MathNet.Numerics.Optimization; using MathNet.Spatial.Euclidean; using Newtonsoft.Json; +using MathNet.Numerics.LinearAlgebra; namespace ESPresense.Models; @@ -14,7 +15,7 @@ public class Scenario(Config? config, ILocate locator, string? name) public int? Confidence { get; set; } public double? Minimum { get; set; } [JsonIgnore] public Point3D LastLocation { get; set; } - public Point3D Location { get; set; } + public Point3D Location { private set; get; } public double? Scale { get; set; } public int? Fixes { get; set; } public string? Name { get; } = name; @@ -26,8 +27,83 @@ public class Scenario(Config? config, ILocate locator, string? name) public DateTime? LastHit { get; set; } public double Probability { get; set; } = 1.0; + // Kalman filter properties + private Matrix? _kalmanStateEstimate; + private Matrix? _kalmanErrorCovariance; + private const double ProcessNoise = 0.01; + private const double MeasurementNoise = 0.1; + private Matrix? _F; // State transition matrix + private Matrix? _H; // Measurement matrix + private Matrix? _Q; // Process noise covariance + private Matrix? _R; // Measurement noise covariance + public bool Locate() { return Locator.Locate(this); } -} \ No newline at end of file + + public void UpdateLocation(Point3D newLocation) + { + if (_kalmanStateEstimate == null || _kalmanErrorCovariance == null) + { + InitializeKalmanFilter(newLocation); + } + + var dt = (DateTime.UtcNow - (LastHit ?? DateTime.UtcNow)).TotalSeconds; + LastHit = DateTime.UtcNow; + + UpdateStateTransitionMatrix(dt); + + // Predict + _kalmanStateEstimate = _F! * _kalmanStateEstimate!; + _kalmanErrorCovariance = _F * _kalmanErrorCovariance! * _F.Transpose() + _Q!; + + // Update + var y = Matrix.Build.DenseOfArray(new double[,] { + { newLocation.X }, { newLocation.Y }, { newLocation.Z } + }) - _H! * _kalmanStateEstimate; + + var S = _H * _kalmanErrorCovariance * _H.Transpose() + _R!; + var K = _kalmanErrorCovariance * _H.Transpose() * S.Inverse(); + + _kalmanStateEstimate += K * y; + _kalmanErrorCovariance = (Matrix.Build.DenseIdentity(6) - K * _H) * _kalmanErrorCovariance; + + // Update Location + Location = new Point3D( + _kalmanStateEstimate[0, 0], + _kalmanStateEstimate[1, 0], + _kalmanStateEstimate[2, 0] + ); + } + + private void InitializeKalmanFilter(Point3D initialLocation) + { + _kalmanStateEstimate = Matrix.Build.DenseOfArray(new double[,] { + { initialLocation.X }, { initialLocation.Y }, { initialLocation.Z }, + { 0 }, { 0 }, { 0 } + }); + _kalmanErrorCovariance = Matrix.Build.DenseDiagonal(6, 6, 1); + + _H = Matrix.Build.DenseOfArray(new double[,] { + { 1, 0, 0, 0, 0, 0 }, + { 0, 1, 0, 0, 0, 0 }, + { 0, 0, 1, 0, 0, 0 } + }); + + _Q = Matrix.Build.DenseDiagonal(6, 6, ProcessNoise); + _R = Matrix.Build.DenseDiagonal(3, 3, MeasurementNoise); + } + + private void UpdateStateTransitionMatrix(double dt) + { + _F = Matrix.Build.DenseOfArray(new double[,] { + { 1, 0, 0, dt, 0, 0 }, + { 0, 1, 0, 0, dt, 0 }, + { 0, 0, 1, 0, 0, dt }, + { 0, 0, 0, 1, 0, 0 }, + { 0, 0, 0, 0, 1, 0 }, + { 0, 0, 0, 0, 0, 1 } + }); + } +}