from Filter import Estimate, KalmanFilter from Sensor import NormalSensor from Observer import KalmanrObserver from numpy import zeros, arange, array import matplotlib.pyplot as plt from cluster_demo import X, P, A, Q, H, R, N_iter estimate=Estimate(X,P) plane=Plane('plane',X) plane.stateSize = A.shape[0] kf=KalmanFilter('kalmanfilter') kf.setPosition(estimate) radar=NormalSensor('radar') radar.measurementSize = H.shape[0] kfObserver=KalmanrObserver('Kalmanr Observer') # allocate space for arrays # Applying the Kalman Filter for k in arange(0, N_iter): kf.predict(A,Q) plane.move(A) radar.detect(plane,H,R) kf.update(radar,H,R) kfObserver.recieve(kf, plane, radar, k) #reporter.result()
from numpy import zeros, arange, array, vstack, hstack, dot import matplotlib.pyplot as plt from sensorRegistration_demo import getG, X, P, A, Q, H, R1, R2, N_iter, position1, bias1, position2, bias2, mu, sigma, E, Rvector1, Rvector2 estimate = Estimate(X,P) biasEstimate = BiasEstimate(X, P, mu) biasEstimate.uncertainty2parameters(mu, sigma) plane=NoisedPlane('plane',X) plane.stateSize = A.shape[0] # snesor 1 kf1=KalmanFilter('Kalman filter for sensor 1') kf1.setPosition(estimate) radar1=BiasSensor('radar 1', position1, bias1) radar1.measurementSize = H.shape[0] kfObserver1=KalmanrObserver('Kalmanr Observer for kf 1') # snesor 2 kf2=KalmanFilter('Kalman filter for sensor 2') kf2.setPosition(estimate) radar2=BiasSensor('radar 2', position2, bias2) radar2.measurementSize = H.shape[0] kfObserver2=KalmanrObserver('Kalmanr Observer for kf 2') #decentralized kf kfdc=KalmanFilterDecentrailized('Kalman filter Centralized') kfdc.setPosition(estimate) kfObserverDC=KalmanrObserverDecentralized('Kalmanr Observer for center') # allocate space for arrays kfdc=KalmanFilterDecentrailized('Kalman filter Centralized')