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
def fusSensors1(Sensors):
# does not work
H_allSensors = np.vstack(Sensors[0].getH(),Sensors[1].getH(),Sensors[2].getH(),Sensors[3].getH(),Sensors[4].getH())
Z_allSensors = np.vstack(Sensors[0].ProduceNoisedMeasuresCoordinations(),Sensors[1].ProduceNoisedMeasuresCoordinations(),
Sensors[2].ProduceNoisedMeasuresCoordinations(),Sensors[3].ProduceNoisedMeasuresCoordinations(),
Sensors[4].ProduceNoisedMeasuresCoordinations())
def createRallSensors():
R = np.zeros(tuple([5*x for x in Sensors[0].createR().shape]))
R[0:2,0:2] = Sensors[0].createR()
R[2:4,2:4] = Sensors[1].createR()
R[4:6,4:6] =Sensors[2].createR()
R[6:8,6:8] = Sensors[3].createR()
R[8:10,8:10] =Sensors[4].createR()
return R
R_allSensors = createRallSensors()
newSensorWithRandH = Sensor(movingObject)
newSensorWithRandH.R = R_allSensors
newSensorWithRandH.H = H_allSensors
newFilter = Filter(newSensorWithRandH)
