parameters: (_title="title", _description="description", _init_x=0.0, _init_y=0.0, _init_z=0.0, _sigma_p_gps=[1.0, 1.0], _sigma_p_dso=[5., 5., 5.], _gps_freq = 1, _sigma_x0 = 0.02, _sigma_v0 = 0.4, _sigma_w_x = 0.2, _sigma_w_v = 0.2)
docstring: print(rmse_dso[:][0])
      print("now comes filtered:")
      print(rmse_filtered[:][0])
docstring_summary: print(rmse_dso[:][0])
      print("now comes filtered:")
      print(rmse_filtered[:][0])
docstring_tokens: print ( rmse_dso [ : ] [ 0 ] ) print ( now comes filtered : ) print ( rmse_filtered [ : ] [ 0 ] )
function: def test_kf_velocity(_title="title", _description="description", _init_x=0.0, _init_y=0.0, _init_z=0.0, _sigma_p_gps=[1.0, 1.0], _sigma_p_dso=[5., 5., 5.], _gps_freq = 1, _sigma_x0 = 0.02, _sigma_v0 = 0.4, _sigma_w_x = 0.2, _sigma_w_v = 0.2):
      N = frames # NumFrames Messpunkte
      T = deltaT # Zeit zwischen Messungen
  
      # Initial Velocities
      vx = 0.0
      vy = 0.0
      vz = 0.0
  
      #sigma_p = np.array([5., 5.])
      sigma_p_gps = np.array(_sigma_p_gps) # std. deviation (x,y) of 1m was set in the VSLAM addon
      sigma_p_dso = np.array(_sigma_p_dso) # std. deviation (x,y,z) is not clear yet, so set pretty high
  
      # simulate measurements
      zGPS, zDSO, x = simulate_measurements_velocity(N,T, vx, vy, vz)
      
      # Kalman Init Parameter:
      x0 = np.array([_init_x, vx, _init_y, vy, _init_z, vz]);    # init measurements
      sigma_x0 = _sigma_x0 #std.dev. for position
      sigma_v0 = _sigma_v0  #std.dev. for velocity
      P0 = np.diag(np.array([sigma_x0, sigma_v0, sigma_x0, sigma_v0, sigma_x0, sigma_v0])**2)
      
      # Parameter der Systemrauschprozesse (Standardabweichungen)
      sigma_w_x = _sigma_w_x #1e-4
      sigma_w_v = _sigma_w_v #1e-10
      
      # System Uebergangsmatrix
      Phik = np.array([[1, T, 0, 0, 0, 0],
                       [0, 1, 0, 0, 0, 0],
                       [0, 0, 1, T, 0, 0],
                       [0, 0, 0, 1, 0, 0],
                       [0, 0, 0, 0, 1, T],
                       [0, 0, 0, 0, 0, 1]], dtype=float)    
      
      # System Rauschkovarianz
      T2 = np.power(T,2)
      T3 = T2 * T
      Qk = np.array([[sigma_w_v*T3/3 + sigma_w_x*T, sigma_w_v*T2/2,   0,                            0,                0,                            0                 ],
                     [sigma_w_v*T2/2,               sigma_w_v*T,      0,                            0,                0,                            0                 ],
                     [0,                            0,                sigma_w_v*T3/3 + sigma_w_v*T, sigma_w_v*T2/2,   0,                            0                 ],
                     [0,                            0,                sigma_w_v*T2/2,               sigma_w_v*T,      0,                            0                 ],
                     [0,                            0,                0,                            0,                sigma_w_v*T3/3 + sigma_w_v*T, sigma_w_v*T2/2    ],
                     [0,                            0,                0,                            0,                sigma_w_v*T2/2,               sigma_w_v*T       ]], dtype=float) 
      # Messmatrix
      # Only measure x and y position (GPS)
      H_GPS = np.array([[1, 0, 0, 0, 0, 0],
                       [0, 0, 1, 0, 0, 0]], dtype=float)
      # Measure x, y and z VELOCITY (DSO)
      H_DSO = np.array([[0, 1, 0, 0, 0, 0],
                       [0, 0, 0, 1, 0, 0],
                       [0, 0, 0, 0, 0, 1]], dtype=float)
      # Messung Rauschkovarianz
      R_GPS = np.diag(sigma_p_gps**2)    
      R_DSO = np.diag(sigma_p_dso**2)    
      
      kf_x = np.zeros((N, 6))
      kf_x[0] = x0
      kf = LinearKalmanFilter(x0, P0)
      
      #Blender: Store initial state vector, covariance matrix and update object
      bpy.types.Scene.KalmanFilter_x.append(kf.x)
      bpy.types.Scene.KalmanFilter_P.append(kf.P)
      objectUpdate(filteredObject, 0, kf.x[0], kf.x[2], kf.x[4]) # (obj, frame, x, y, z)
      
      # Run Kalman filter:
      for frame in np.arange(1, N):
          #print("Kalman Filter step:", frame)
          kf.predict(Phik, Qk, T)
          
          if _title == "GPS":
              # Only use GPS as measurements
              if(frame % _gps_freq == 0):
                  kf.update(zGPS[frame, :], H_GPS, R_GPS)
          elif _title == "DSO":
              # Only use DSO as measurements (indices 1, 3, 5 for vx,vy,vz)
              kf.update(zDSO[frame, 1::2], H_DSO, R_DSO)
          else:
              # Fuse GPS and DSO
              # -> GPS: Only take new Measurements every 5th iteration:
              if(frame % _gps_freq == 0):
                  kf.update(zGPS[frame, :], H_GPS, R_GPS)
              # -> DSO: Take new Measurements every timestep:
              kf.update(zDSO[frame, 1::2], H_DSO, R_DSO)
              
          
          # Speichere aktuellen Zustand
          kf_x[frame, :] = kf.x
          
          # Update filtered Blenderobject
          objectUpdate(filteredObject, frame, kf.x[0], kf.x[2], kf.x[4]) # (obj, frame, x, y, z)
          
          # Visualize Covariance Matrix
          visualizeCovariance(kf.x, kf.P)
          
          # Store P Matrix from KalmanFilter for display
          bpy.types.Scene.KalmanFilter_x.append(kf.x)
          bpy.types.Scene.KalmanFilter_P.append(kf.P)
          
      # Remove all assigned handler functions:
      bpy.app.handlers.frame_change_pre.clear()
      # Now register a handler function to update the 3D Data Plot
      bpy.app.handlers.frame_change_pre.append(frameUpdate)
      
      # Save file for plotting via matplotlib
      rmse_gps = np.zeros((N, 2))
      rmse_dso = np.zeros((N, 3))
      rmse_filtered = np.zeros((N, 3))
      rmse_velocity = np.zeros((N, 3))
      
      for frame in np.arange(0, N):
          rmse_gps[frame][0] = zGPS[frame][0] - x[frame][0]
          rmse_gps[frame][1] = zGPS[frame][1] - x[frame][2]
          rmse_dso[frame][0] = zDSO[frame][0] - x[frame][0]
          rmse_dso[frame][1] = zDSO[frame][2] - x[frame][2]
          rmse_dso[frame][2] = zDSO[frame][4] - x[frame][4]
          rmse_filtered[frame][0] = kf_x[frame][0] - x[frame][0]
          rmse_filtered[frame][1] = kf_x[frame][2] - x[frame][2]
          rmse_filtered[frame][2] = kf_x[frame][4] - x[frame][4]
          # Velocity
          rmse_velocity[frame][0] = kf_x[frame][1] - x[frame][1]
          rmse_velocity[frame][1] = kf_x[frame][3] - x[frame][3]
          rmse_velocity[frame][2] = kf_x[frame][5] - x[frame][5]
      
      '''
      print(rmse_dso[:][0])
      print("now comes filtered:")
      print(rmse_filtered[:][0])
      '''
      
      plotResults(N, x, kf_x, zGPS, zDSO, rmse_gps, rmse_dso, rmse_filtered, rmse_velocity, _title, _description, _init_x, _init_y, _init_z, _sigma_p_gps, _sigma_p_dso, _gps_freq, _sigma_x0, _sigma_v0, _sigma_w_x, _sigma_w_v)
function_tokens: def test_kf_velocity ( _title = "title" , _description = "description" , _init_x = 0.0 , _init_y = 0.0 , _init_z = 0.0 , _sigma_p_gps = [ 1.0 , 1.0 ] , _sigma_p_dso = [ 5. , 5. , 5. ] , _gps_freq = 1 , _sigma_x0 = 0.02 , _sigma_v0 = 0.4 , _sigma_w_x = 0.2 , _sigma_w_v = 0.2 ) : N = frames # NumFrames Messpunkte T = deltaT # Zeit zwischen Messungen # Initial Velocities vx = 0.0 vy = 0.0 vz = 0.0 #sigma_p = np.array([5., 5.]) sigma_p_gps = np . array ( _sigma_p_gps ) # std. deviation (x,y) of 1m was set in the VSLAM addon sigma_p_dso = np . array ( _sigma_p_dso ) # std. deviation (x,y,z) is not clear yet, so set pretty high # simulate measurements zGPS , zDSO , x = simulate_measurements_velocity ( N , T , vx , vy , vz ) # Kalman Init Parameter: x0 = np . array ( [ _init_x , vx , _init_y , vy , _init_z , vz ] ) # init measurements sigma_x0 = _sigma_x0 #std.dev. for position sigma_v0 = _sigma_v0 #std.dev. for velocity P0 = np . diag ( np . array ( [ sigma_x0 , sigma_v0 , sigma_x0 , sigma_v0 , sigma_x0 , sigma_v0 ] ) ** 2 ) # Parameter der Systemrauschprozesse (Standardabweichungen) sigma_w_x = _sigma_w_x #1e-4 sigma_w_v = _sigma_w_v #1e-10 # System Uebergangsmatrix Phik = np . array ( [ [ 1 , T , 0 , 0 , 0 , 0 ] , [ 0 , 1 , 0 , 0 , 0 , 0 ] , [ 0 , 0 , 1 , T , 0 , 0 ] , [ 0 , 0 , 0 , 1 , 0 , 0 ] , [ 0 , 0 , 0 , 0 , 1 , T ] , [ 0 , 0 , 0 , 0 , 0 , 1 ] ] , dtype = float ) # System Rauschkovarianz T2 = np . power ( T , 2 ) T3 = T2 * T Qk = np . array ( [ [ sigma_w_v * T3 / 3 + sigma_w_x * T , sigma_w_v * T2 / 2 , 0 , 0 , 0 , 0 ] , [ sigma_w_v * T2 / 2 , sigma_w_v * T , 0 , 0 , 0 , 0 ] , [ 0 , 0 , sigma_w_v * T3 / 3 + sigma_w_v * T , sigma_w_v * T2 / 2 , 0 , 0 ] , [ 0 , 0 , sigma_w_v * T2 / 2 , sigma_w_v * T , 0 , 0 ] , [ 0 , 0 , 0 , 0 , sigma_w_v * T3 / 3 + sigma_w_v * T , sigma_w_v * T2 / 2 ] , [ 0 , 0 , 0 , 0 , sigma_w_v * T2 / 2 , sigma_w_v * T ] ] , dtype = float ) # Messmatrix # Only measure x and y position (GPS) H_GPS = np . array ( [ [ 1 , 0 , 0 , 0 , 0 , 0 ] , [ 0 , 0 , 1 , 0 , 0 , 0 ] ] , dtype = float ) # Measure x, y and z VELOCITY (DSO) H_DSO = np . array ( [ [ 0 , 1 , 0 , 0 , 0 , 0 ] , [ 0 , 0 , 0 , 1 , 0 , 0 ] , [ 0 , 0 , 0 , 0 , 0 , 1 ] ] , dtype = float ) # Messung Rauschkovarianz R_GPS = np . diag ( sigma_p_gps ** 2 ) R_DSO = np . diag ( sigma_p_dso ** 2 ) kf_x = np . zeros ( ( N , 6 ) ) kf_x [ 0 ] = x0 kf = LinearKalmanFilter ( x0 , P0 ) #Blender: Store initial state vector, covariance matrix and update object bpy . types . Scene . KalmanFilter_x . append ( kf . x ) bpy . types . Scene . KalmanFilter_P . append ( kf . P ) objectUpdate ( filteredObject , 0 , kf . x [ 0 ] , kf . x [ 2 ] , kf . x [ 4 ] ) # (obj, frame, x, y, z) # Run Kalman filter: for frame in np . arange ( 1 , N ) : #print("Kalman Filter step:", frame) kf . predict ( Phik , Qk , T ) if _title == "GPS" : # Only use GPS as measurements if ( frame % _gps_freq == 0 ) : kf . update ( zGPS [ frame , : ] , H_GPS , R_GPS ) elif _title == "DSO" : # Only use DSO as measurements (indices 1, 3, 5 for vx,vy,vz) kf . update ( zDSO [ frame , 1 : : 2 ] , H_DSO , R_DSO ) else : # Fuse GPS and DSO # -> GPS: Only take new Measurements every 5th iteration: if ( frame % _gps_freq == 0 ) : kf . update ( zGPS [ frame , : ] , H_GPS , R_GPS ) # -> DSO: Take new Measurements every timestep: kf . update ( zDSO [ frame , 1 : : 2 ] , H_DSO , R_DSO ) # Speichere aktuellen Zustand kf_x [ frame , : ] = kf . x # Update filtered Blenderobject objectUpdate ( filteredObject , frame , kf . x [ 0 ] , kf . x [ 2 ] , kf . x [ 4 ] ) # (obj, frame, x, y, z) # Visualize Covariance Matrix visualizeCovariance ( kf . x , kf . P ) # Store P Matrix from KalmanFilter for display bpy . types . Scene . KalmanFilter_x . append ( kf . x ) bpy . types . Scene . KalmanFilter_P . append ( kf . P ) # Remove all assigned handler functions: bpy . app . handlers . frame_change_pre . clear ( ) # Now register a handler function to update the 3D Data Plot bpy . app . handlers . frame_change_pre . append ( frameUpdate ) # Save file for plotting via matplotlib rmse_gps = np . zeros ( ( N , 2 ) ) rmse_dso = np . zeros ( ( N , 3 ) ) rmse_filtered = np . zeros ( ( N , 3 ) ) rmse_velocity = np . zeros ( ( N , 3 ) ) for frame in np . arange ( 0 , N ) : rmse_gps [ frame ] [ 0 ] = zGPS [ frame ] [ 0 ] - x [ frame ] [ 0 ] rmse_gps [ frame ] [ 1 ] = zGPS [ frame ] [ 1 ] - x [ frame ] [ 2 ] rmse_dso [ frame ] [ 0 ] = zDSO [ frame ] [ 0 ] - x [ frame ] [ 0 ] rmse_dso [ frame ] [ 1 ] = zDSO [ frame ] [ 2 ] - x [ frame ] [ 2 ] rmse_dso [ frame ] [ 2 ] = zDSO [ frame ] [ 4 ] - x [ frame ] [ 4 ] rmse_filtered [ frame ] [ 0 ] = kf_x [ frame ] [ 0 ] - x [ frame ] [ 0 ] rmse_filtered [ frame ] [ 1 ] = kf_x [ frame ] [ 2 ] - x [ frame ] [ 2 ] rmse_filtered [ frame ] [ 2 ] = kf_x [ frame ] [ 4 ] - x [ frame ] [ 4 ] # Velocity rmse_velocity [ frame ] [ 0 ] = kf_x [ frame ] [ 1 ] - x [ frame ] [ 1 ] rmse_velocity [ frame ] [ 1 ] = kf_x [ frame ] [ 3 ] - x [ frame ] [ 3 ] rmse_velocity [ frame ] [ 2 ] = kf_x [ frame ] [ 5 ] - x [ frame ] [ 5 ] plotResults ( N , x , kf_x , zGPS , zDSO , rmse_gps , rmse_dso , rmse_filtered , rmse_velocity , _title , _description , _init_x , _init_y , _init_z , _sigma_p_gps , _sigma_p_dso , _gps_freq , _sigma_x0 , _sigma_v0 , _sigma_w_x , _sigma_w_v )