notebook.community

Edit and run 





In [103]:



    


# Loading Visualization

output the estimation

//output the estimation outfile << ukf.x_(0) << "\t"; //pos1 - est outfile << ukf.x_(1) << "\t"; //pos2 - est outfile << ukf.x_(2) << "\t"; //vel_abs -est outfile << ukf.x_(3) << "\t"; //yaw_angle -est outfile << ukf.x_(4) << "\t"; //yaw_rate -est

    //output the measurements
    if (measurement_pack_list[k].sensor_type_
            == MeasurementPackage::LASER) {
        //output the estimation
        out_file_ << measurement_pack_list[k].raw_measurements_(0) << "\t"; //p1 - meas
        out_file_ << measurement_pack_list[k].raw_measurements_(1) << "\t"; //p2 - meas
    } else if (measurement_pack_list[k].sensor_type_
            == MeasurementPackage::RADAR) {
        //output the estimation in the cartesian coordinates
        float ro = measurement_pack_list[k].raw_measurements_(0);
        float phi = measurement_pack_list[k].raw_measurements_(1);
        out_file_ << ro * cos(phi) << "\t"; //p1_meas
        out_file_ << ro * sin(phi) << "\t"; //p2_meas
    }

    //output the gt packages
    out_file_ << gt_pack_list[k].gt_values_(0) << "\t"; //p1 - GT
    out_file_ << gt_pack_list[k].gt_values_(1) << "\t"; //p2 - GT
    out_file_ << gt_pack_list[k].gt_values_(2) << "\t"; //v_abs - GT
    out_file_ << gt_pack_list[k].gt_values_(3) << "\t"; //yaw - GT
    out_file_ << gt_pack_list[k].gt_values_(4) << "\t"; //yaw_dot - GT
    out_file_ << gt_pack_list[k].gt_values_(5) << "\t"; //v1 - GT
    out_file_ << gt_pack_list[k].gt_values_(6) << "\n"; //v2 - GT




In [85]:



    


import plotly.offline as py
from plotly.graph_objs import *
import pandas as pd
import math
py.init_notebook_mode()

#we aim to visualize the following params: 
#'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'

#input file fields as they are saved into the UKF output file
#my_cols=['time_stamp', 'p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt', 'NIS_laser', 'NIS_radar']

my_cols=['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v1_gt','v2_gt', 'NIS_laser', 'NIS_radar']
with open('../output/obj_pose-laser-radar-synthetic-input.txt') as f:
    table_ekf_output = pd.read_table(f, sep='\t', header=None, names=my_cols, lineterminator='\n')

Cool





In [86]:



    


#check the parsed file
table_ekf_output[0:5]



    


















    
Out[86]:










  
    

      
      p1est
      p2est
      vest
      yawest
      yawrateest
      p1meas
      p2meas
      p1
      p2
      v1_gt
      v2_gt
      NIS_laser
      NIS_radar
    

  
  
    

      0
      0.312243
      0.580340
      0.00000
      0.000000e+00
      0.000000e+00
      0.312243
      0.580340
      0.600000
      0.600000
      5.19994
      0.000000
      0.000000
      0.00000
    

    

      1
      0.680375
      0.601438
      5.66596
      5.071250e-18
      -5.442260e-18
      0.862916
      0.534212
      0.859997
      0.600045
      5.19975
      0.001797
      0.000000
      117.19600
    

    

      2
      1.144750
      0.505139
      5.67540
      -1.171270e-01
      -9.608100e-03
      1.173850
      0.481073
      1.119980
      0.600225
      5.19943
      0.005390
      0.481637
      0.00000
    

    

      3
      1.276830
      0.531742
      5.66101
      6.359230e-02
      1.804380e-02
      0.969149
      0.397513
      1.379960
      0.600629
      5.19898
      0.010778
      0.000000
      1.82532
    

    

      4
      1.593150
      0.578483
      5.67106
      1.051850e-01
      2.968770e-02
      1.650630
      0.624690
      1.639900
      0.601347
      5.19839
      0.017960
      0.428738
      0.00000
    

  




















In [ ]:

Visualize and compare the estimated/measurement/groun truth poses in 2D





In [87]:



    


import plotly.offline as py
from plotly.graph_objs import *

#Ground Truth
trace1 = Scatter(
    x=table_ekf_output['p1'],
    y=table_ekf_output['p2'],
    xaxis='x2',
    yaxis='y2',
    name = 'ground truth position',
    mode = 'markers'      
)


#estimations
trace2 = Scatter(
    x=table_ekf_output['p1est'],
    y=table_ekf_output['p2est'],
    xaxis='x2',
    yaxis='y2',
    name='UKF position estimation',
    mode = 'markers'       
)

#Measurements
trace3 = Scatter(
    x=table_ekf_output['p1meas'],
    y=table_ekf_output['p2meas'],
    xaxis='x2',
    yaxis='y2',
    name = 'position measurements',
    #mode = 'markers'
)


data = [trace1, trace2, trace3]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='px in m'
    ),
    yaxis2=dict(
    
        anchor='y2',
        title='py in m'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')

Now let's swithc to something serious





In [ ]:

Compare estimated Px and Ground Truth Px





In [82]:



    


import plotly.offline as py
from plotly.graph_objs import *

t = list(range(0,len(table_ekf_output)))
## we want: 'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'
#['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt']


trace1 = Scatter(
    x=t,
    y=table_ekf_output['p1'],
    xaxis='x2',
    yaxis='y2',
    name = 'px ground truth in m',
    #mode = 'markers'
)

trace2= Scatter(
    x=t,
    y=table_ekf_output['p1est'],
    xaxis='x2',
    yaxis='y2',
    name = 'px estimated in m ',
    #mode = 'markers'
)

trace3 = Scatter(
    x=t,
    y=table_ekf_output['p1meas'],
    xaxis='x2',
    yaxis='y2',
    name = 'px measured in m ',
    #mode = 'markers'

)



data = [trace1, trace2, trace3]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='t'
    ),
    yaxis2=dict(
    
        anchor='y2',
        title='m'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')

Compare estimated Velocity and Ground Truth Velocity

Note: Ground Truth values must be present for Velocity in given data file for this to work. My input file does not have ground truth values for Velocity





In [28]:



    


import plotly.offline as py
from plotly.graph_objs import *

t = list(range(0,len(table_ekf_output)))
## we want: 'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'
#['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt']


trace7 = Scatter(
    x=t,
    y=table_ekf_output['v'],
    xaxis='x2',
    yaxis='y2',
    name = 'ground truth velocity ',
    #mode = 'markers'
)

trace8 = Scatter(
    x=t,
    y=table_ekf_output['vest'],
    xaxis='x2',
    yaxis='y2',
    name = 'estimated velocity ',
    #mode = 'markers'
)



data = [trace7, trace8]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='t'
    ),
    yaxis2=dict(
    
        anchor='y2',
        title='m/s'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')



    


















    






---------------------------------------------------------------------------
KeyError                                  Traceback (most recent call last)
/Users/ramiz/anaconda/envs/carnd-term1/lib/python3.5/site-packages/pandas/indexes/base.py in get_loc(self, key, method, tolerance)
   2133             try:
-> 2134                 return self._engine.get_loc(key)
   2135             except KeyError:

pandas/index.pyx in pandas.index.IndexEngine.get_loc (pandas/index.c:4433)()

pandas/index.pyx in pandas.index.IndexEngine.get_loc (pandas/index.c:4279)()

pandas/src/hashtable_class_helper.pxi in pandas.hashtable.PyObjectHashTable.get_item (pandas/hashtable.c:13742)()

pandas/src/hashtable_class_helper.pxi in pandas.hashtable.PyObjectHashTable.get_item (pandas/hashtable.c:13696)()

KeyError: 'v'

During handling of the above exception, another exception occurred:

KeyError                                  Traceback (most recent call last)
<ipython-input-28-9e72a58022be> in <module>()
      9 trace7 = Scatter(
     10     x=t,
---> 11     y=table_ekf_output['v'],
     12     xaxis='x2',
     13     yaxis='y2',

/Users/ramiz/anaconda/envs/carnd-term1/lib/python3.5/site-packages/pandas/core/frame.py in __getitem__(self, key)
   2057             return self._getitem_multilevel(key)
   2058         else:
-> 2059             return self._getitem_column(key)
   2060 
   2061     def _getitem_column(self, key):

/Users/ramiz/anaconda/envs/carnd-term1/lib/python3.5/site-packages/pandas/core/frame.py in _getitem_column(self, key)
   2064         # get column
   2065         if self.columns.is_unique:
-> 2066             return self._get_item_cache(key)
   2067 
   2068         # duplicate columns & possible reduce dimensionality

/Users/ramiz/anaconda/envs/carnd-term1/lib/python3.5/site-packages/pandas/core/generic.py in _get_item_cache(self, item)
   1384         res = cache.get(item)
   1385         if res is None:
-> 1386             values = self._data.get(item)
   1387             res = self._box_item_values(item, values)
   1388             cache[item] = res

/Users/ramiz/anaconda/envs/carnd-term1/lib/python3.5/site-packages/pandas/core/internals.py in get(self, item, fastpath)
   3541 
   3542             if not isnull(item):
-> 3543                 loc = self.items.get_loc(item)
   3544             else:
   3545                 indexer = np.arange(len(self.items))[isnull(self.items)]

/Users/ramiz/anaconda/envs/carnd-term1/lib/python3.5/site-packages/pandas/indexes/base.py in get_loc(self, key, method, tolerance)
   2134                 return self._engine.get_loc(key)
   2135             except KeyError:
-> 2136                 return self._engine.get_loc(self._maybe_cast_indexer(key))
   2137 
   2138         indexer = self.get_indexer([key], method=method, tolerance=tolerance)

pandas/index.pyx in pandas.index.IndexEngine.get_loc (pandas/index.c:4433)()

pandas/index.pyx in pandas.index.IndexEngine.get_loc (pandas/index.c:4279)()

pandas/src/hashtable_class_helper.pxi in pandas.hashtable.PyObjectHashTable.get_item (pandas/hashtable.c:13742)()

pandas/src/hashtable_class_helper.pxi in pandas.hashtable.PyObjectHashTable.get_item (pandas/hashtable.c:13696)()

KeyError: 'v'

Compare estimated Yaw Angle and Ground Truth Yaw Angle

Note: Ground Truth values must be present for Yaw Angle in given data file for this to work. My input file does not have ground truth values for Yaw Angle





In [89]:



    


import plotly.offline as py
from plotly.graph_objs import *

t = list(range(0,len(table_ekf_output)))
## we want: 'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'
#['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt']


trace7 = Scatter(
    x=t,
    y=table_ekf_output['yaw'],
    xaxis='x2',
    yaxis='y2',
    name = 'ground truth yaw angle ',
    #mode = 'markers'
)

trace8 = Scatter(
    x=t,
    y=table_ekf_output['yawest'],
    xaxis='x2',
    yaxis='y2',
    name = 'estimated yaw angle ',
    #mode = 'markers'
)



data = [trace7, trace8]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='t'
    ),
    yaxis2=dict(
    
        anchor='y2',
        title='rad'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')

Compare estimated Yaw Rate and Ground Truth Yaw Rate

Note: Ground Truth values must be present for Yaw Rate in given data file for this to work. My input file does not have ground truth values for Yaw Rate





In [88]:



    


import plotly.offline as py
from plotly.graph_objs import *

t = list(range(0,len(table_ekf_output)))
## we want: 'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'
#['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt']


trace7 = Scatter(
    x=t,
    y=table_ekf_output['yawrate'],
    xaxis='x2',
    yaxis='y2',
    name = 'ground truth yaw rate ',
    #mode = 'markers'
)

trace8 = Scatter(
    x=t,
    y=table_ekf_output['yawrateest'],
    xaxis='x2',
    yaxis='y2',
    name = 'estimated yaw rate',
    #mode = 'markers'
)



data = [trace7, trace8]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='t'
    ),
    yaxis2=dict(
    
        anchor='y2',
        title='rad/s'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')

Check Consistency of Filter for Laser (Laser NIS)





In [83]:



    


import plotly.offline as py
from plotly.graph_objs import *
import matplotlib.pyplot as plt

t = list(range(0,len(table_ekf_output)))

## we want: 'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'
#['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt']


trace2 = Scatter(
    x=t,
    y=table_ekf_output['NIS_laser'],
    xaxis='x2',
    yaxis='y2',
    name = 'NIS laser',
    #mode = 'markers'
)


trace1= Scatter(
    x=[t[0], t[-1]],
    y=[5.991, 5.991],
    xaxis='x2',
    yaxis='y2',
    name = '95 %',
    #mode = 'markers'
)



data = [trace1, trace2]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='k'
    ),
    yaxis2=dict(
    
        anchor='y2',
        #title='py'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')

Check Consistency of Filter for Radar (Radar NIS)





In [84]:



    


import plotly.offline as py
from plotly.graph_objs import *

t = list(range(0,len(table_ekf_output)))
## we want: 'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'
#['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt']


trace2 = Scatter(
    x=t,
    y=table_ekf_output['NIS_radar'],
    xaxis='x2',
    yaxis='y2',
    name = 'NIS radar',
    #mode = 'markers'
)


trace1= Scatter(
    x=[t[0], t[-1]],
    y=[7.815, 7.815],
    xaxis='x2',
    yaxis='y2',
    name = '95 %',
    #mode = 'markers'
)



data = [trace1, trace2]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='k'
    ),
    yaxis2=dict(
    
        anchor='y2',
        #title='py'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')

Compare All Values Estimations and Ground Truth Values Combined

Note: Ground Truth values must be present for all values in given data file for this to work. My input file does not have ground truth values for all values





In [83]:



    


import plotly.offline as py
from plotly.graph_objs import *

t = list(range(0,len(table_ekf_output)))
## we want: 'p1', 'p1est','p1meas', 'p2', 'p2est','p2meas', 'v', 'vest', 'yaw', 'yawest', 'yawrate', 'yawrateest', 'acc', 'yawacc'
#['p1est','p2est','vest','yawest','yawrateest','p1meas','p2meas','p1','p2','v','yaw', 'yawrate','v1_gt','v2_gt']


trace1 = Scatter(
    x=t,
    y=table_ekf_output['p1'],
    xaxis='x2',
    yaxis='y2',
    name = 'p1',
    #mode = 'markers'
)

trace2= Scatter(
    x=t,
    y=table_ekf_output['p1est'],
    xaxis='x2',
    yaxis='y2',
    name = 'p1est',
    #mode = 'markers'
)

trace3 = Scatter(
    x=t,
    y=table_ekf_output['p1meas'],
    xaxis='x2',
    yaxis='y2',
    name = 'p1meas',
    #mode = 'markers'
)

trace4= Scatter(
    x=t,
    y=table_ekf_output['p2'],
    xaxis='x2',
    yaxis='y2',
    name = 'p2',
    #mode = 'markers'
)

trace5 = Scatter(
    x=t,
    y=table_ekf_output['p2est'],
    xaxis='x2',
    yaxis='y2',
    name = 'p2est',
    #mode = 'markers'
)

trace6= Scatter(
    x=t,
    y=table_ekf_output['p2meas'],
    xaxis='x2',
    yaxis='y2',
    name = 'p2meas',
    #mode = 'markers'
)

trace7 = Scatter(
    x=t,
    y=table_ekf_output['v'],
    xaxis='x2',
    yaxis='y2',
    name = 'v',
    #mode = 'markers'
)

trace8 = Scatter(
    x=t,
    y=table_ekf_output['vest'],
    xaxis='x2',
    yaxis='y2',
    name = 'vest',
    #mode = 'markers'
)

trace9= Scatter(
    x=t,
    y=table_ekf_output['yaw'],
    xaxis='x2',
    yaxis='y2',
    name = 'yaw',
    #mode = 'markers'
)

trace10 = Scatter(
    x=t,
    y=table_ekf_output['yawest'],
    xaxis='x2',
    yaxis='y2',
    name = 'yawest',
    #mode = 'markers'
)

trace11= Scatter(
    x=t,
    y=table_ekf_output['yawrate'],
    xaxis='x2',
    yaxis='y2',
    name = 'yawrate',
    #mode = 'markers'
)

trace12= Scatter(
    x=t,
    y=table_ekf_output['yawrateest'],
    xaxis='x2',
    yaxis='y2',
    name = 'yawrateest',
    #mode = 'markers'
)

data = [trace1, trace2, trace3, trace4, trace5, trace6, trace7, trace8, trace9, trace10, trace11, trace12]

layout = Layout(
    xaxis2=dict(
   
        anchor='x2',
        title='t'
    ),
    yaxis2=dict(
    
        anchor='y2',
        #title='py'
    )
)

fig = Figure(data=data, layout=layout)
py.iplot(fig, filename= 'EKF')



    


















    
























In [ ]:



    






    











In [ ]:

Content source: informramiz/Unscented-Kalman-Filter

Similar notebooks:

notebook.community | gallery | about