#NEWLINE# def sensor_fusion_test( wheel_sigma = 2.0 , gps_sigma = 4.0 ) : #NEWLINE# #INDENT# dt = 0.1 #NEWLINE# kf2 = KalmanFilter ( dim_x = 2 , dim_z = 2 ) #NEWLINE# kf2 . F = array ( [ [ 1.0 , dt ] , [ 0.0 , 1.0 ] ] ) #NEWLINE# kf2 . H = array ( [ [ 1.0 , 0.0 ] , [ 1.0 , 0.0 ] ] ) #NEWLINE# kf2 . x = array ( [ [ 0.0 ] , [ 0.0 ] ] ) #NEWLINE# kf2 . Q = ( array ( [ [ ( ( dt ** 3 ) / 3 ) , ( ( dt ** 2 ) / 2 ) ] , [ ( ( dt ** 2 ) / 2 ) , dt ] ] ) * 0.02 ) #NEWLINE# kf2 . P *= 100 #NEWLINE# kf2 . R [ ( 0 , 0 ) ] = ( wheel_sigma ** 2 ) #NEWLINE# kf2 . R [ ( 1 , 1 ) ] = ( gps_sigma ** 2 ) #NEWLINE# random . seed ( SEED ) #NEWLINE# xs = [ ] #NEWLINE# zs = [ ] #NEWLINE# nom = [ ] #NEWLINE# for i in range ( 1 , 100 ) : #NEWLINE# #INDENT# m0 = ( std_dev + ( randn ( ) * wheel_sigma ) ) #NEWLINE# m1 = ( i + ( randn ( ) * gps_sigma ) ) #NEWLINE# if ( gps_sigma > 1e+40 ) : #NEWLINE# #INDENT# m1 = ( - 1e+40 ) #NEWLINE# #UNINDENT# z = array ( [ [ m0 ] , [ m1 ] ] ) #NEWLINE# kf2 . predict ( ) #NEWLINE# kf2 . update ( z ) #NEWLINE# xs . append ( kf2 . x . T [ 0 ] ) #NEWLINE# zs . append ( z . T [ 0 ] ) #NEWLINE# nom . append ( i ) #NEWLINE# #UNINDENT# xs = asarray ( xs ) #NEWLINE# zs = asarray ( zs ) #NEWLINE# nom = asarray ( nom ) #NEWLINE# res = ( nom - xs [ : , 0 ] ) #NEWLINE# std_dev = np . std ( res ) #NEWLINE# print ( 'fusion std: {:.3f}' . format ( np . std ( res ) ) ) #NEWLINE# if DO_PLOT : #NEWLINE# #INDENT# plt . subplot ( 211 ) #NEWLINE# plt . plot ( xs [ : , 0 ] ) #NEWLINE# plt . subplot ( 212 ) #NEWLINE# plt . axhline ( 0 ) #NEWLINE# plt . plot ( res ) #NEWLINE# plt . show ( ) #NEWLINE# #UNINDENT# print ( kf2 . Q ) #NEWLINE# print ( kf2 . K ) #NEWLINE# return std_dev