class SubDepthSensor:
currentDepth = 0
# Relative to the MSL
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 UpdateValue(self, connection, dataFile, SaveSensorData):
# Create a method for the sensor to be turned off
readSensor = True
while (readSensor):
if (self.sensor.read()):
self._filter.Step(numpy.matrix([0]),
numpy.matrix([self.sensor.depth()]))
self.currentDepth = self._filter.GetCurrentState()[0, 0]
connection.send(self.currentDepth)
if (SaveSensorData):
dataFile.write(
str(time.time()) + "," + str(self.currentDepth) + "\n")
if (connection.poll()):
readSensor = False
dataFile.close()
#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')
pylab.ylabel('Y position')
pylab.title('Fusion of two measurements')
pylab.legend(('true', 'measured 1', 'measured 2', 'kalman'))
pylab.show()
