def __init__(self, ip, port):
KalmanFinalAgent.__init__(self, ip, port)
self._initializePF()
self._initializeOcc()
self.kalmanFilter1 = KalmanFilter(self.NOISE)
self.kalmanFilter2 = KalmanFilter(self.NOISE)
def __init__(self, ip, port):
PFFinalAgent.__init__(self, ip, port)
# world details
self.SHOT_SPEED = float(self.constants['shotspeed'])
self.target1 = (0, 0, False)
self.target2 = (0, 0, False)
self.delta = 0.0
self.kalmanFilter1 = KalmanFilter(self.NOISE)
self.kalmanFilter2 = KalmanFilter(self.NOISE)
def simulator_setup(nbr_of_robots, start_positions):
# Create pose handlers
# subscribe pose handlers to Rosaria pose
# give correct offset if several
# Create KalmanFilters
# subscribe them to pose handlers
# Create robot handlers
# subscribe them to KalmanFilter
# make robots move towards their startPoint
robots = [RobotHandler(0), RobotHandler(1)]
for i in range(0, nbr_of_robots - 1):
offsetX = 0.0
offsetY = 0.0
if i == 1:
offsetY = 1.0
h = PoseHandler(i, offsetX, offsetY)
h_sub = rospy.Subscriber("RosAria" + str(i) + "/pose", Odometry,
h.measure)
kf = KalmanFilter(i)
kf_sub = rospy.Subscriber("Sensor" + str(i) + "/measurement", Odometry,
kf.new_measurement)
#robots[i] = RobotHandler(i)
r_sub = rospy.Subscriber("Filter" + str(i) + "/state", Odometry,
robots[i].update_state)
print len(robots)
return robots
def initializeKalmanFilter():
A = np.array([[1, 0], [0, 1]])
B = np.array([[1, 0], [0, 1]])
Q = 20 * np.array([[1, 0], [0, 1]])
H = np.array([[1, 0], [0, 1]])
R = 10 * np.array([[1, 0], [0, 1]])
return kfilter.KalmanFilter(A, B, Q, H, R)
def __init__(self, dataFile, connection, SaveSensorData):
# Create an instance of the sensor object
self.sensor = ms5837.MS5837_30BA()
# Check if we were able to connect to the sensor
if (self.sensor._bus == None):
# There is no depth sensor connected
return None
else:
# A is the matrix we need to create that converts the last state to the new one, in our case it's just 1 because we get depth measurements directly
A = numpy.matrix([1])
# H is the matrix we need to create that converts sensors readings into state variables, since we get the readings directly from the sensor this is 1
H = numpy.matrix([1])
# B is the control matrix, since this is a 1D case and because we have no inputs that we can change we can set this to zero.
B = 0
# Q is the process covariance, since we want accurate values we set this to a very low value
Q = numpy.matrix([0.001])
# R is the measurement covariance, using a conservative estimate of 0.1 is fair
R = numpy.matrix([0.1])
# IC is the original prediction of the depth, setting this to normal room conditions makes sense
IC = self.currentDepth
# P is the initial prediction of the covariance, setting it to 1 is reasonable
P = numpy.matrix([1])
# We must initialize the sensor before reading it
self.sensor.init()
# Create the filter
self._filter = KalmanFilter(A, B, H, IC, P, Q, R)
# Create and start the process
p = Process(target=self.UpdateValue,
args=(connection, dataFile, SaveSensorData))
p.start()
def KF_demo_run(self):
model = self.model
truth_X = self.truth_X
meas_Z = self.meas_Z
estimated_X = self.estimated_X
m_update = self.m_update
P_update = self.P_update
targetstate = self.targetstate
for k in range(self.N_duration):
# simulate dynamic target state
# targetstate = model["F"].dot(targetstate) + np.sqrt(model["Q"]).dot(np.random.randn(targetstate.shape[0],targetstate.shape[1])) # generate ground truth for states of a target
W = model["sigma_v"] * model["B"].dot(np.random.randn(model["B"].shape[1], targetstate.shape[1])) # equivalent to 'np.sqrt(model["Q"]).dot(np.random.randn(targetstate.shape[0],targetstate.shape[1])) '
targetstate = model["F"].dot(targetstate) + W # generate ground truth for states of a target
truth_X.append(targetstate)
# simulate target measurement
# np.sqrt(model["R"]).dot(np.random.randn(model["R"].shape[1],1)) meas_zk = model["H"].dot(targetstate) + np.sqrt(model["R"]).dot(np.random.randn(model["R"].shape[1],1)) # generate measurement of a target
V = model["D"].dot(np.random.randn(model["D"].shape[1], targetstate.shape[1])) # equivalent to 'np.sqrt(model["R"]).dot(np.random.randn(model["R"].shape[1],1)) '
meas_zk = model["H"].dot(targetstate) + V # generate measurement of a target
meas_Z.append(meas_zk)
# Call KalmanFilter function
m_update, P_update = KalmanFilter(meas_zk, model, m_update, P_update)
estimated_X.append(m_update)
return truth_X, meas_Z, estimated_X
def __init__(self, gui=None, title=None):
super(FilterModule, self).__init__(gui=gui, title=title)
self.movingAverageModule = MovingAverageFilter(gui=gui,
mainModule=self)
self.kalmanFilterToy = KalmanFilterToy(gui=gui, mainModule=self)
self.kalmanFilter = KalmanFilter(gui=gui, mainModule=self)
self.font = ('Comic Sans MS', 11)
self.noisyImage = tk.PhotoImage(file="images/noisy_data.png")
def add_filter(self, id, site_name, filter_state=None):
if site_name not in self.filter_map[id].filters:
if filter_state is None:
self.filter_map[id].filters[
site_name] = KalmanFilter.KalmanFilter(
self.filter_map[id].configuration, site_name)
else:
self.filter_map[id].filters[site_name] = filter_state
def __init__(self, i2cBus = 1):
self.gyro = Gyroscope(i2cBus, Gyroscope.DPS2000)
self.accel = Accelerometer(i2cBus, Accelerometer.SCALE_A_8G)
initOrientation = self.accel.getOrientation()
self.roll = initOrientation.roll
self.pitch = initOrientation.pitch
self.yaw = initOrientation.yaw
self.kalmanFilter = KalmanFilter(IMU.GYRO_NOISE, IMU.BIAS_NOISE, IMU.ACCEL_NOISE)
self.thread = threading.Thread(target=self.updateOrientation)
self.thread.daemon = True
self.thread.start()
def __init__(self, sensor_id):
self.cache = []
self.sensor_id = sensor_id
self.readings = []
r_std, q_std = 1., 0.003
self.kf = KalmanFilter(dim_x=2, dim_z=1)
self.kf.x = np.array([[19], [0]]) # position, velocity
self.kf.F = np.array([[1, 1], [0, 1]])
self.kf.R = np.array([[r_std**2]]) # input noise
self.kf.H = np.array([[1., 0.]])
self.kf.P = np.diag([0.001**2, 0])
self.kf.Q = Q_discrete_white_noise(2, 1, q_std**2)
self.last_est = 0
def __init__(self, ip, port):
PFAgent.__init__(self, ip, port)
# world details
self.SHOT_SPEED = float(self.constants['shotspeed'])
self.target = (0, 0, False)
self.delta = 0.0
self.kalmanFilter = KalmanFilter(self.NOISE)
#visualization init
init_window(self.worldHalfSize * 2, self.worldHalfSize * 2)
self.resetPredictionGrid()
def real_world_setup(nbr_of_robots):
# Create UWBHandlers
# This is done from the commandline
# Create KalmanFilters
# Subscribe to UWBHandlers
# Create RobotHandlers
# Suscribe them to KalmanFilters
# Make them move towards start_positions
robots = []
for i in range(0, nbr_of_robots - 1):
kf = KalmanFilter(i)
kf_sub = rospy.Subscriber("Sensor" + str(i) + "/measurement", Odometry,
kf.new_measurement)
robots[i] = RobotHandler(i)
r_sub = rospy.Subscriber("Filter" + str(i) + "/state", Odometry,
robots[i].update_state)
return robots
def __init__(self, object_id, frame_number, indexes, H, pixelToMeters):
self.object_id = object_id
self.indexes = indexes
self.current_frame = frame_number
self.frames = [self.current_frame]
self.top_left = (min(self.indexes[1]), min(self.indexes[0]))
self.bottom_right = (max(self.indexes[1]), max(self.indexes[0]))
self.width = self.bottom_right[0] - self.top_left[0]
self.height = self.bottom_right[1] - self.top_left[1]
self.current_centroid = (sum(self.indexes[0]) / len(self.indexes[0]),
sum(self.indexes[1]) / len(self.indexes[1]))
self.centroids = [self.current_centroid]
self.kalman_filter = kf.KalmanFilter(self.object_id,
self.current_frame,
self.current_centroid)
self.found = True
self.speed = 40.0
self.speeds = [self.speed]
self.H = H
self.pixelToMeters = pixelToMeters
def __init__(self, id, obj_coord, nFrame):
self.id = id
self.nFrame = nFrame
self.visible = True
self.finished = False
minx = min(obj_coord[0])
maxx = max(obj_coord[0])
miny = min(obj_coord[1])
maxy = max(obj_coord[1])
self.bbox = np.array([minx, maxx, miny, maxy])
self.center = [(self.bbox[1]-self.bbox[0])/2+self.bbox[0], (self.bbox[3]-self.bbox[2])/2+self.bbox[2]]
self.posList = [self.center]
self.bboxList = [self.bbox]
self.framesList = [self.nFrame]
self.velocities = [50]
self.kalman = KalmanFilter(self.center)
class DepthSensor: currentDepth = 0; # Relative to the MSL self.sensor = ms5837.MS5837_30BA() # A is the matrix we need to create that converts the last state to the new one, in our case it's just 1 because we get depth measurements directly A = numpy.matrix([1]) # H is the matrix we need to create that converts sensors readings into state variables, since we get the readings directly from the sensor this is 1 H = numpy.matrix([1]) # B is the control matrix, since this is a 1D case and because we have no inputs that we can change we can set this to zero. B = 0 # Q is the process covariance, since we want accurate values we set this to a very low value Q = numpy.matrix([0.00001]) # R is the measurement covariance, using a conservative estimate of 0.1 is fair R = numpy.matrix([0.1]) # IC is the original prediction of the depth, setting this to normal room conditions makes sense IC = self.currentDepth # P is the initial prediction of the covariance, setting it to 1 is reasonable P = numpy.matrix([1]) # We must initialize the sensor before reading it self.sensor.init() # Create the filter self._filter = KalmanFilter(A,B,H,IC,P,Q,R)
SystemCovNoise[1,1] = varDiffPrice
# Measurement error Covariance Matrix
# The covariance matrix of the measurement error observed at a given time t.
# We want to compute both price and return, so we obtain both
# therefore the noise measurement matrix is a 2x2 covariance matrix
MeasurCovNoise = np.eye(Nd) # Independent noise. This is not really true I think though. Can we check it ?
MeasurCovNoise[0,0] = varNoisePrice
MeasurCovNoise[1,1] = varNoiseDiff
######################################################################
############ USE THE KF !! #######
######################################################################
Ntst = 10
myKF = KF.KalmanFilter(A = A,B = B,C = C,
SystemCovNoise = SystemCovNoise, MeasurCovNoise = MeasurCovNoise)
Yhat,SigmaXXhatList,Ypred, SigmaXXpredList = myKF.fit(dates, Matrix_tr)
Ypredtest,SigmaXXpredtestList = myKF.predict(Ntst = Ntst)
######################################################################
## Extract the data to plot
RealPrice = Matrix_tr[:,[0]]
EstimatedPrice = Yhat[:,[0]]
PredictedPrice = Ypred[:,[0]]
sigmaXhatList = ul.fnp([ x[0,0] for x in SigmaXXhatList])
sigmaXpredList= ul.fnp([ x[0,0] for x in SigmaXXpredList])
sigmaXhatList = np.sqrt(sigmaXhatList )
SigmaXpredList = np.sqrt(sigmaXpredList)
sigmaYhatList = np.sqrt(sigmaXhatList**2 + MeasurCovNoise[0,0])
self._auto_run()
else:
self._manual_run()
if __name__ == '__main__':
print 'Testing FilterManager...'
origin_pos = np.array([200, 0, np.pi/2])
origin_cov = np.eye(3) * 3 # Initial Covariance
origin_cov[2,2] = 0.02
# Need to call this or gtk will never release locks
gobject.threads_init()
fm = FilterManager()
run_kf = False
run_pf = False
#run_kf = True
run_pf = True
if run_kf:
fm.add_filter(KalmanFilter.KalmanFilter(origin_pos, origin_cov))
if run_pf:
fm.add_filter(ParticleFilter.ParticleFilter(origin_pos, origin_cov))
tg = TestGUI(fm.get_draw())
#tt = TestThread(fm, tg, auto=True)
tt = TestThread(fm, tg, auto=False)
tt.start()
tg.mainloop()
tt.quit = True
import numpy as np
import matplotlib.pyplot as plt
import KalmanFilter as kf
import data.fakeData as fakeData
if __name__ == '__main__':
accelerations = fakeData.generateCurve((3, 1000))
true_velocity, noisy_velocity = fakeData.genVelocity((3, 1000))
dimension = 3
A = np.identity(dimension)
P = np.identity(dimension)
kf = kf.KalmanFilter(A, P, dimension)
Q = np.zeros((3, 3))
### TODO ###
i = 0
state = []
while (i < len(noisy_velocity[0])):
kf.predictState(accelerations[:, i], 1, Q)
kf.getKalmanGain()
kf.update(noisy_velocity[:, 1])
i += 1
state.append(kf.v_k)
state = np.transpose(np.array(state))
for x in range(dimension):
plt.plot(state[x])
speedY = muzzle_velocity * math.sin(angle * math.pi / 180)
# This is our guess of the initial state. I intentionally set the Y value
# wrong to illustrate how fast the Kalman filter will pick up on that.
X0 = numpy.matrix([[0], [500], [speedX], [speedY]])
#P0 = numpy.eye(4)
P0 = numpy.matrix([[1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
Q = numpy.zeros(4)
#R = numpy.eye(4)*0.2
#R = numpy.matrix([[0.2, 0, 0, 0], [0, 0, 0.2, 0], [0, 0.2, 0, 0], [0, 0, 0, 0.2]])
R = 0.2 * numpy.eye(6)
iterations = len(x)
kf = KalmanFilter(A, B, H, X0, P0, Q, R)
kx = []
ky = []
# Iterate through the simulation.
for i in range(iterations):
kx.append(kf.GetCurrentState()[0, 0])
ky.append(kf.GetCurrentState()[1, 0])
kf.Step(
U,
numpy.matrix([[nx1[i]], [ny1[i]], [nx2[i]], [ny2[i]], [vx[i]],
[vy[i]]]))
# Plot all the results we got.
pylab.plot(x, y, 'r-', nx1, ny1, 'y:', nx2, ny2, 'g--', kx, ky, 'b.-')
pylab.xlabel('X position')
import numpy as np
from KalmanFilter import *
from Simulation import *
from sympy import *
x, y = symbols('x y')
def functions():
f_x = 100 * sin(x)
f_y = -100 * cos(y)
return (f_x, f_y)
if __name__ == "__main__":
fncs = functions()
print "\nAcceleration functions:\nf(x) = " + str(fncs[0])
print "f(y) = " + str(fncs[1])
print "\n"
kalman = KalmanFilter(true_initial_state=np.array([10, 10, 2,
2]).reshape(4, 1),
number_of_iters=50,
acceleration_function_x=fncs[0],
acceleration_function_y=fncs[1],
sig_acceleration=np.array([2.5, 2.5]))
sim = Simulation(true_trajectory=kalman.get_true_trajectory(),
estimation=kalman.get_estimation(),
difference=kalman.get_difference(),
delta_t=kalman.get_delta_t())
def main():
print("Start systemmodel main")
timestep = 0.01
car1 = Vechile(26, timestep, x=0, y=0, gamma=math.pi / 4)
car1Err = Vechile(26, timestep, x=0, y=0, gamma=math.pi / 4)
alpha_a, alpha_a_err, accel_a, accel_a_err = testSecvenceGenerate(timestep)
# The vechile position and oriantation without error
X_car1 = []
Y_car1 = []
Gamma_car1 = []
W_car1 = [0]
# The vechile position and oriantation with error
X_car1Err = []
Y_car1Err = []
Gamma_car1Err = []
W_car1Err = [0]
# The vechile position and oriantation with error
X_car1ErrF = []
Y_car1ErrF = []
Gamma_car1ErrF = []
W_car1ErrF = [0]
plt.figure()
ax = plt.subplot(111, aspect='equal')
X_car1ErrF.append(0)
Y_car1ErrF.append(0)
Gamma_car1ErrF.append(math.pi / 4)
X_car1Err.append(0)
Y_car1Err.append(0)
Gamma_car1Err.append(math.pi / 4)
X_car1.append(0)
Y_car1.append(0)
Gamma_car1.append(math.pi / 4)
# State covariance
P = getStateCovariance()
Q = getProcessNoise(0.0, timestep, math.pi / 180 * 100, 26, 1.0)
R = getMeasurmentNoise(velf=0.00001, groErr=math.pi / 360 / 5)
# e1 = plotHelp.plotEllipse(0, 0, P[1:3, 1:3])
# ax.add_artist(e1)
l_kalmanFilter = KalmanFilter(car1Err.state, car1Err.f, car1Err.h,
car1Err.F_x, car1Err.F_u, car1Err.H, P, Q, R,
Vechile.State, Vechile.Output)
Velf = [0]
VelfErr = [0]
VelfErrF = [0]
for accel, alpha in zip(accel_a, alpha_a):
l_input = Vechile.Input(accel, alpha)
newState = car1.f(car1.state, l_input)
l_measurment = car1.h(car1.state)
car1.state = newState
# Position without error
X_car1.append(newState.x)
Y_car1.append(newState.y)
Gamma_car1.append(newState.gamma)
Velf.append(newState.vf)
W_car1.append(newState.w)
Vf_err, W_err = testhelp.generateMesError(Velf, 0.0, 0.0, W_car1, 0.00,
math.pi / 360 / 5)
for accel_err, alpha_err, vf_mes, w_mes in zip(accel_a_err, alpha_a_err,
Vf_err, W_err):
l_measurment = Vechile.Output(vf_mes, w_mes)
l_input = Vechile.Input(accel_err, alpha_err)
newStateErr = car1Err.f(car1Err.state, l_input)
car1Err.state = newStateErr
X_car1Err.append(newStateErr.x)
Y_car1Err.append(newStateErr.y)
Gamma_car1Err.append(newStateErr.gamma)
VelfErr.append(newStateErr.vf)
W_car1Err.append(newStateErr.w)
newStateErrF, l_P = l_kalmanFilter.predict(l_kalmanFilter.X, l_input,
l_kalmanFilter.P)
# print('Angle', P[3, 3])
newStateErrF, l_P = l_kalmanFilter.update(newStateErrF, l_measurment,
l_P)
l_kalmanFilter.X = newStateErrF
l_kalmanFilter.P = l_P
X_car1ErrF.append(newStateErrF.x)
Y_car1ErrF.append(newStateErrF.y)
Gamma_car1ErrF.append(newStateErrF.gamma)
VelfErrF.append(newStateErrF.vf)
W_car1ErrF.append(newStateErrF.w)
plt.plot(X_car1, Y_car1, '--ro')
plt.plot(X_car1Err, Y_car1Err, '--go')
plt.plot(X_car1ErrF, Y_car1ErrF, '--bo')
plt.legend(['Real', 'Error', 'Filtered'])
plt.figure()
plt.title('Angle speed')
plt.plot(W_car1)
plt.plot(W_err)
plt.plot(W_car1Err)
plt.plot(W_car1ErrF)
plt.legend(['Real', 'Added Error', 'Error based pred.', 'Filtered'])
plt.figure()
plt.title('Oriantation')
plt.plot(Gamma_car1)
plt.plot(Gamma_car1Err)
plt.plot(Gamma_car1ErrF)
plt.legend(['Real', 'Error', 'Filtered'])
plt.figure()
plt.title('Velocity')
plt.plot(Velf)
plt.plot(Vf_err)
plt.plot(VelfErr)
plt.plot(VelfErrF)
plt.legend(['Real', 'Added Error', 'Error based pred.', 'Filtered'])
posErr = []
posErrF = []
for x_p, y_p, x_pf, y_pf, x_pR, y_pR in zip(X_car1, Y_car1, X_car1ErrF,
Y_car1ErrF, X_car1Err,
Y_car1Err):
p = complex(x_p, y_p)
pf = complex(x_pf, y_pf)
pr = complex(x_pR, y_pR)
errorF = abs(p - pf)
error = abs(p - pr)
posErr.append(error)
posErrF.append(errorF)
# #
plt.figure()
plt.plot(posErr)
plt.plot(posErrF)
plt.legend(['Error', 'ErrorF'])
plt.show()
print("End systemmodel main")
+ SEP + b'pit' + KEY + bytearray(struct.pack('f', pitch))
+ SEP + b'rol' + KEY + bytearray(struct.pack('f', roll)))
tot_ax = 0
tot_ay = 0
tot_az = 0
tot_rz = 0
tot_ry = 0
tot_rx = 0
calib_runs = 0
TOTAL_CALIBRATION_RUNS = 1000
notified_calibration = False
kf = KalmanFilter(cooldown, 2, 0.01, 0.5)
predictions = []
measurements = []
try:
while 1:
if calibration: # runs for n loops
if not notified_calibration: # this statement runs once.
print('Calibrating...')
notified_calibration = True
# calibrate acceleration
acc = imu.readAccel()
tot_ax += acc['x']
tot_ay += acc['y']
tot_az += acc['z'] - 1
import cv2 as cv
import params as prm
import util as util
import KalmanFilter as kalman
kalman = kalman.KalmanFilter() #Se inicializa la clase del filtro de Kalman
#cap = cv.VideoCapture(cv.samples.findFile("Videos/videoPeq2.mp4"))
# cap = cv.VideoCapture(cv.samples.findFile("Videos/gido_completo.mp4"))
# cap = cv.VideoCapture(cv.samples.findFile("Videos/tomi1.mp4"))
#cap = cv.VideoCapture(cv.samples.findFile("pendulo_tobi.mp4"))
#cap = cv.VideoCapture(cv.samples.findFile("Videos/car.mp4")) #DESCOMENTAR LINEAS DE ABAJO
#cap = cv.VideoCapture(0)
#for i in range(250): #DESCOMENTAR PARA EL VIDEO CAR
#frame = cap.read() #(tiene un titulo al principio)
lower_thr, upper_thr = [], []
if prm.COLOR_ALGORITHM is True:
prev, prev_gray, x, y, w, h, lower_thr, upper_thr = util.captureROI(
cap) #Se captura por primera vez la seleccion del usuario
else:
prev, prev_gray, x, y, w, h = util.captureROI(cap)
prev = util.space_translate(
x, y, prev
) #Se aplica la trasformacion de las coordenadas de las features del espacio
#de la seleccion al espacio del frame.
frame_num = 0
error = False
dyn_h = h
def range_of (self, pos):
""" returns tuple containing noisy range data to A and B
given a position 'pos'
"""
ra = sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
rb = sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)
return (ra + random.randn()*self.noise_factor,
rb + random.randn()*self.noise_factor)
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
import random
from KalmanFilter import *
from aisettings import *
kalman_filter = KalmanFilter(VAR, EST_VAR)
for i in range(5, 50):
num = random.randint(-10, 10)
kalman_filter.input_latest_noisy_measurement(num)
filtered_value = kalman_filter.get_latest_estimated_measurement()
print("Estimated:\t%s\t|\tActual:\t%s" % (num, filtered_value))
plt.grid()
# estimation de la position par un filtre de Kalman
# -----------------------------------------------------
# dimensions
# ******* A MODIFIER EN TD *****
nx = 1
nu = 1
ny = 1
# ******************************
positionKF = kf.KalmanFilter(nx, nu, ny)
# matrices représentant l'équation d'état et de mesure
# ******* A MODIFIER EN TD *****
Ak = np.array( [[1.0]] )
Bk = np.array( [[Te]] )
# ******************************
positionKF.setStateEquation(Ak, Bk)
# matrice représentant l'équation de mesure
# ******* A MODIFIER EN TD *****
Ck = np.array( [[1.0]])
# ******************************
positionKF.setCk(Ck)
weather_df['Measurement Timestamp'], infer_datetime_format=True)
weather_df.index = weather_df['Measurement Timestamp']
weather_df = weather_df.drop(['Measurement Timestamp'], axis=1)
stations = weather_df['Station Name'].unique()
for s in stations:
curr_df = weather_df[weather_df['Station Name'] == s][[
'Air Temperature', 'Wet Bulb Temperature', 'Humidity',
'Rain Intensity', 'Wind Speed', 'Barometric Pressure',
'Solar Radiation', 'Battery Life'
]]
print(s)
kf = KalmanFilter.KalmanFilter(dataframe=curr_df)
kf.filter()
fig, (temp_ax, err_ax) = plt.subplots(nrows=2, ncols=1, figsize=(15, 10))
err_ax.plot(range(len(kf.post_fit_residuals['Humidity'][-500:])),
kf.post_fit_residuals['Humidity'][-500:],
color='red',
label='Estimated Humidity Residuals')
err_ax.legend()
temp_ax.plot(range(len(curr_df['Humidity'][-500:])),
curr_df['Humidity'][-500:],
'--',
label='Measured Humidity')
temp_ax.plot(range(len(kf.state_estimates['Humidity'][-500:])),
