예제 #1
0
    def __init__(self, algorithm):
        super(Ball2DTracker, self).__init__()
        self.threshold_filter = ThresholdFilter(
            np.array([20, 150, 50], dtype=np.uint8),
            np.array([40, 255, 255], dtype=np.uint8))
        self.algo = algorithm()

        # Setup the Kalman Filter noise matrices
        self.Q = (10**-3) * np.eye(4, 4)
        self.R = (10**-1) * np.eye(2, 2)
        self.kalman_filter = KalmanFilter(self.Q, self.R)
        self.centroid_algo = Centroid()
        self.transformed_image = np.copy(self.image)
        self.centroid = self.centroid_algo(
            self.threshold_filter(self.transformed_image))
예제 #2
0
from Object import Plane
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 Observer import KalmanrObserver, KalmanrObserverDecentralized, VBbiasObserverDecentralized

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