Covariance calculation on initial setState.

s1st · Oct 4, 2014 · 0196832 · 0196832
1 parent dca5c0a
commit 0196832
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 14 deletions.
diff --git a/app/src/main/java/com/villoren/android/kalmanlocationmanager/lib/LooperThread.java b/app/src/main/java/com/villoren/android/kalmanlocationmanager/lib/LooperThread.java
@@ -157,7 +157,7 @@ public void onLocationChanged(final Location location) {
             if (mLatitudeTracker == null) {
 
                 mLatitudeTracker = new Tracker1D(TIME_STEP, COORDINATE_NOISE);
-                mLatitudeTracker.setState(position, 0.0);
+                mLatitudeTracker.setState(position, 0.0, noise);
             }
 
             mLatitudeTracker.update(position, noise);
@@ -169,7 +169,7 @@ public void onLocationChanged(final Location location) {
             if (mLongitudeTracker == null) {
 
                 mLongitudeTracker = new Tracker1D(TIME_STEP, COORDINATE_NOISE);
-                mLongitudeTracker.setState(position, 0.0);
+                mLongitudeTracker.setState(position, 0.0, noise);
             }
 
             mLongitudeTracker.update(position, noise);
@@ -183,7 +183,7 @@ public void onLocationChanged(final Location location) {
                 if (mAltitudeTracker == null) {
 
                     mAltitudeTracker = new Tracker1D(TIME_STEP, ALTITUDE_NOISE);
-                    mAltitudeTracker.setState(position, noise);
+                    mAltitudeTracker.setState(position, 0.0, noise);
                 }
 
                 mAltitudeTracker.update(position, noise);

diff --git a/app/src/main/java/com/villoren/android/kalmanlocationmanager/lib/Tracker1D.java b/app/src/main/java/com/villoren/android/kalmanlocationmanager/lib/Tracker1D.java
@@ -34,7 +34,7 @@ class Tracker1D {
     /**
      * Time step
      */
-    private final double mt, mt2, mt2d2;
+    private final double mt, mt2, mt2d2, mt3d2, mt4d4;
 
     /**
      * Process noise covariance
@@ -59,21 +59,19 @@ class Tracker1D {
      */
     public Tracker1D(double timeStep, double processNoise) {
 
-        // Lookup
+        // Lookup time step
         mt = timeStep;
         mt2 = mt * mt;
         mt2d2 = mt2 / 2.0;
-
-        // Time step
-        double t3 = mt2 * timeStep;
-        double t4 = t3 * timeStep;
+        mt3d2 = mt2 * mt / 2.0;
+        mt4d4 = mt2 * mt2 / 4.0;
 
         // Process noise covariance
-        double pn2 = processNoise * processNoise;
-        mQa = pn2 * t4 / 4.0;
-        mQb = pn2 * t3 / 2.0;
+        double n2 = processNoise * processNoise;
+        mQa = n2 * mt4d4;
+        mQb = n2 * mt3d2;
         mQc = mQb;
-        mQd = pn2 * mt2;
+        mQd = n2 * mt2;
 
         // Estimated covariance
         mPa = mQa;
@@ -89,11 +87,20 @@ public Tracker1D(double timeStep, double processNoise) {
      *
      * @param position
      * @param velocity
+     * @param noise
      */
-    public void setState(double position, double velocity) {
+    public void setState(double position, double velocity, double noise) {
 
+        // State vector
         mXa = position;
         mXb = velocity;
+
+        // Covariance
+        double n2 = noise * noise;
+        mPa = n2 * mt4d4;
+        mPb = n2 * mt3d2;
+        mPc = mPb;
+        mPd = n2 * mt2;
     }
 
     /**