plot rng dot std dev and tune range finder noise parameters

Author: bresch

range_finder_kinematic_consistency/range_finder_consistency_check_replay.py

@@ -45,26 +45,28 @@ def run(logfile):
     (t, rng, vz, vz_var) = getAllData(logfile)
 
     rng_kin = RangeFinderConsistencyCheck()
-    rng_kin._vel_bottom_gate = 0.2
+    rng_kin._vel_bottom_gate = 1.0
 
     n = len(t)
     inn = [0.0]
     rng_var = [0.0]
     test_ratio = [0.0]
     signed_test_ratio_lpf = [0.0]
+    vel_bottom_var = [0.0]
     is_consistent = [False]
 
     for k in range(1, n):
-        rng_var.append(0.1**2 + (0.05*rng[k])**2)
+        rng_var.append(0.01**2 + (0.01*rng[k])**2)
         rng_kin.update(rng[k], rng_var[k], vz[k], vz_var[k], t[k]*1e6)
         inn.append(rng_kin.getInnov())
         test_ratio.append(rng_kin._vel_bottom_test_ratio)
         signed_test_ratio_lpf.append(rng_kin.getSignedTestRatioLpf())
         is_consistent.append(rng_kin.isKinematicallyConsistent())
+        vel_bottom_var.append(rng_kin._vel_bottom_innov_var - rng_var[k])
 
-    plotData(t, rng, rng_var, is_consistent, vz, inn, test_ratio, signed_test_ratio_lpf)
+    plotData(t, rng, rng_var, is_consistent, vz, inn, test_ratio, signed_test_ratio_lpf, vel_bottom_var)
 
-def plotData(t, rng, rng_var, is_consistent, vz, inn, test_ratio, signed_test_ratio_lpf):
+def plotData(t, rng, rng_var, is_consistent, vz, inn, test_ratio, signed_test_ratio_lpf, vel_bottom_var):
     n_plots = 4
     ax1 = plt.subplot(n_plots, 1, 1)
     ax1.plot(t, rng)
@@ -82,7 +84,10 @@ def plotData(t, rng, rng_var, is_consistent, vz, inn, test_ratio, signed_test_ra
 
     ax3 = plt.subplot(n_plots, 1, 3, sharex=ax1)
     ax3.plot(t, vz)
-    ax3.plot(t, vz+inn)
+    rng_dot = vz+inn
+    ax3.plot(t, rng_dot)
+    ax3.plot(t, rng_dot+np.sqrt(vel_bottom_var), 'r--')
+    ax3.plot(t, rng_dot-np.sqrt(vel_bottom_var), 'r--')
     ax3.set_ylim(-5, 5)
     ax3.legend(["vz", "rng_dot"])