def fusSensors2(Sensors): R_Sensors = np.array([Sensors[0].createR(),Sensors[1].createR(),Sensors[2].createR(),Sensors[3].createR(),Sensors[4].createR()]) def createRfromSensors(R_Sensors): R2 = R_Sensors.copy() R2 = [la.inv(matrix) for matrix in R2] R2 = sum(R2) R2 = la.inv(R2) return R2 R= createRfromSensors(R_Sensors) newSensorWithR = Sensor(movingObject) newSensorWithR.R = R newFilter = Filter(newSensorWithR) newTrajectory = np.zeros(Trajectory.T.shape) for i in range(m): for j in range(5): newTrajectory[i] += la.inv(R_Sensors[j]) @ Z_Sensors[j].T[i] newTrajectory[i] = R @ newTrajectory[i] newTrajectory = newTrajectory.T X_2ndStrategy = np.zeros((6,1)) allX_2ndStrategy= np.zeros((m,6,1)) # save Xs P_2ndStrategy = sensor.createPo(10) for i in range(m): centerPredicted, pPredicted = newFilter.Predict(X_2ndStrategy,P_2ndStrategy) X_2ndStrategy,P_2ndStrategy = newFilter.Filter(centerPredicted,pPredicted,(newTrajectory.T)[i]) allX_2ndStrategy[i]=X_2ndStrategy rx_2ndStrategy_f = list(allX[:,0,:1].T.flat) ry_2ndStrategy_f = list(allX[:,1,:2].T.flat) plt.scatter(rx_2ndStrategy_f,ry_2ndStrategy_f) # print filtered predictions
F = sensor.createF() D = sensor.createD() """ Produce all filters and predictions for visualization""" X = np.zeros((6,1)) allX= np.zeros((m,6,1)) # save Xs P = sensor.createPo(10) Trajectory = sensor.ProduceTrajectoryUsingAAndR() myFilter.filteredCenters.extend([X]) myFilter.filteredPs.extend([P]) for i in range(m): centerPredicted,pPredicted = myFilter.Predict(X,P) X,P = myFilter.Filter(centerPredicted,pPredicted,(Trajectory.T)[i]) allX[i]=X rx_f = list(allX[:,0,:1].T.flat) ry_f = list(allX[:,1,:2].T.flat) """print filtered""" plt.scatter(rx_f,ry_f, s=10, label = "after filtering") # print filtered predictions plt.legend(loc='lower right') # Retrodiction Retrocidted_Xs = np.zeros(allX.shape) Xcopy = X.copy() for i in range(m):