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):
