def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
g = 9.8 # gravitational constant
f1 = kf.KalmanFilter(dim_x=5, dim_z=2)
ay = .5*dt**2
f1.F = np.array ([[1, dt, 0, 0, 0], # x = x0+dx*dt
[0, 1, 0, 0, 0], # dx = dx
[0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 1]]) # ddy = -g.
f1.H = np.array([
[1, 0, 0, 0, 0],
[0, 0, 1, 0, 0]])
f1.R *= r
f1.Q *= q
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.array([[x,vx,y,vy,-9.8]]).T
return f1
def univariate_kalman(rssi_data): """The following function is used to settle on a value for each point for the initial radio map readings. This is intended to be a replacement or alternative to the fingerprint_average() function in the data_fetch module. **Parameters** rssi_data:1xn list is the list containing the RSSI values sent from the reader when taking measurements at a single coordinate.""" # calculate the sample standard deviation of the rssi_data so for the # measurement noise value standard_deviation = statistics.stdev(rssi_data) # one dimension for the Kalman filter - data across time at one point is # being examined reader = kalman.KalmanFilter(dim_x=1, dim_z=1) # initial estimate for rssi value at point (μ) reader.x = np.array([[50]]) # the state is not expected to change so STM is 1 reader.F = np.array([[1]]) # measurement function ensures that residual is calculated using common # quantities reader.H = np.array([[1]]) # no conversion necessary between units """ The covariance matrix is the property which describes how two or more vairables vary with each other. The main diagonal is filled with the variances of each of the variables, while the other positions are the covariance of the two quantities' axis which the value sits on. As covariance is commutable, the matrix will be symmetrical about the main diagonal """ # covariance matrix - the initial position is unknown so the matrix is # initialised with a large value indicating large variance (and an almost # uniform distribution) reader.P *= 1000 # measurement noise (aka measurement noise) reader.R = np.array(standard_deviation) # process noise - use the provided white noise generator as used in the # example in the documentation # https://filterpy.readthedocs.org/en/latest/kalman/KalmanFilter.html # reader.Q = Q_discrete_white_noise(dim=2, dt=3) # discrete noise method does not seem to work for univariate problems for rssi in rssi_data: # no control vector needed (argument u) reader.predict() reader.update(rssi) return reader.x
def initialize_filter(self):
print("Initializing filters")
# define the limits of a vinerow, normalized to center vinerow
# the data I receive now, is not centered to tractor center
# thats why the x limits are now non default
self.limits.static.center_x_max = -5
self.limits.static.center_x_min = -11
self.limits.static.center_y_max = 1.5
self.limits.static.center_y_min = -1.5
self.limits.static.center_z_max = 0.5
self.limits.static.center_z_min = -0.5
self.limits.static.direction_x_max = 1
self.limits.static.direction_x_min = -1
self.limits.static.direction_y_max = 1
self.limits.static.direction_y_min = -1
self.limits.static.direction_z_max = 1
self.limits.static.direction_z_min = -1
self.limits.static.distance_x_max = 110
self.limits.static.distance_x_min = -110
self.limits.dynamic.center_x = 1
self.limits.dynamic.center_y = 0.05
self.limits.dynamic.center_z = 0.05
self.limits.dynamic.direction_x = 0.05
self.limits.dynamic.direction_y = 0.05
self.limits.dynamic.direction_z = 0.05
self.limits.dynamic.distance_x = 0.20
# create a list of 7 kalman filters, one for each vine row
for i in range(7):
self.kfs.append(fp.KalmanFilter(dim_x=7, dim_z=7))
# initialize all the filters
for i, kf in enumerate(self.kfs):
kf.x = np.zeros(7)
kf.F = np.array([[1., 0., 0., 0., 0., 0., 0.],
[0., 1., 0., 0., self.gh.speed * self.runtime[i].time_step, 0., 0.],
[0., 0., 1., 0., 0, self.gh.speed * self.runtime[i].time_step, 0.],
[0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 0., 1., 0.],
[0., 0., 0., self.gh.speed * self.runtime[i].time_step, 0., 0., 1.]])
kf.H = np.diag(np.full(7, 1))
kf.P = np.diag(np.array([100., 100., 100., 0.64, 0.64, 0.64, 400.]))
kf.R = np.diag(np.array([10., 10., 10., 0.3, 0.3, 0.3, 15.]), 0)
kf.Q = np.diag(np.array([10., 10., 10., 0.3, 0.3, 0.3, 15.]), 0)
kf.B = np.zeros((7, 7))
def __init__(self, Q=None):
self._filter = kf.KalmanFilter(dim_x=3, dim_z=3)
self._filter.F = self._filter.H = np.eye(3)
if Q is None:
self._filter.Q = np.diag([.01, .01, .01])
else:
assert Q.shape == (3, 3)
self._filter.Q = Q
self._classes = None
self._inited = False
def _init_filter(self, val0: float, ts0: float, val1: float, ts1: float):
'''
ts = timestamp.
'''
dt = ts1 - ts0 #Velocity
self._filter = kf.KalmanFilter(dim_x=2, dim_z=1)
self._filter.x = np.array([val0, dt
]) #Initial state of position and velocity
self._filter.F = np.array([[1.,dt], \
[0.,1.]])# state transition matrix
self._filter.H = np.array([[1., 0.]]) # Measurement function
# self._filter.P = np.array([[val1, 0.],[0.,dt]]) # covariance matrix with known starting positions set
self._filter.P *= 500. # covariance matrix
self._filter.R = self.SENSOR_NOMINAL_RANGE # state uncertainty
self._filter.Q = self.MEASURE_NOISE_VARIANCE #(2,.1,.1) #Q_discrete_white_noise(2, dt, .1) # process uncertainty
# self._filter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=self.MEASURE_NOISE_VARIANCE)
return
def EKFTest():
# load test data
x, y, H, phi = loadData(model="accmove", f=non_linearF, df=dF)
# add noise term to expectation output to obtain actual observation
actual_obv = np.mat(np.zeros([y.shape[0], y.shape[1]]))
for i in range(y.shape[1]):
actual_obv[:, i] = y[:, i] + np.mat(
np.random.normal(0, 100, y.shape[0])).T
print("x:", x)
print("y:", y)
print("actual:", actual_obv.shape)
# generate noise model
R, Q = generate_Noise(3, model="linear", sigma=0.01)
print("R:", R)
print("Q:", Q)
kf_obj = KF.KalmanFilter(dim_x=y.shape[1], dim_z=y.shape[1])
kf_obj.H = H
kf_obj.F = phi
kf_obj.Q = Q
kf_obj.R = R
kf_obj.z = actual_obv
estimated_v = []
# predict data
for k in range(y.shape[0]):
# input the control state vector
if k + 1 >= y.shape[0]:
i = k
else:
i = k + 1
t = x[i]
# Kalman filter
kf_obj.x = y[i, :].T
kf_obj.update(z=actual_obv[k, :].T)
kf_obj.predict()
estimated_v.append(kf_obj.x_prior)
# print("estimate:",estimated_v[-1])
pass
# plot data
# estimated_v : filtered prediction output
# y: expected output
# actual_obv: actual observation data
plot_data(x, y, np.mat(np.array(estimated_v)), actual_obv)
def __init__(self,
init: Union[ObjectTarget3D, TrackingTarget3D],
Q: np.ndarray = np.eye(3)):
'''
:param init: initial state for the target
:param Q: system process noise
'''
self._filter = kf.KalmanFilter(dim_x=3, dim_z=3)
# initialize matrices
self._filter.F = self._filter.H = np.eye(3)
self._filter.Q = np.asarray(Q).reshape(3, 3)
# feed input
self._filter.x = init.dimension
self._filter.P = init.dimension_var
self._saved_tag = init.tag
def filterPyIteration(Net, iterations):
f = km.KalmanFilter(dim_x=Net.X_Vector.size, dim_z=Net.Z_Vector.size)
f.x = Net.X_Vector.T
f.F = Net.A_Matrix.todense()
f.H = Net.H_Matrix
f.P = Net.P_Matrix.todense()
f.R = Net.R_Matrix
f.Q = Net.Q_Matrix.todense()
Xs = np.zeros((Iterations, f.x.size))
Ps = np.zeros((Iterations, f.x.size, f.x.size))
Xs[0, :] = np.array(f.x)
Ps[0, :, :] = np.array(f.P)
for i in range(1, int(Iterations)):
#f.x = f.x.T
f.predict()
try:
f.update(Net.MeasurementData[:, i])
except:
print('It sort of didnt wwork')
f.x = f.x.T
Xs[i, :] = np.array(f.x)[:, 0]
Ps[i, :, :] = np.array(f.P)
Net.H_Matrix[i, Net.nodal_CPs[Measurement_Nodes[i]]] = 1
Net.TransposeH = Net.H_Matrix.transpose()
#Net.MeasurementData = np.random.uniform(99,101,Iterations)
#Net.MeasurementData = np.sin(np.arange(Iterations)*0.01)+100
Net.MeasurementData = np.vstack(
(np.load('MeasureData1.npy'), np.load('MeasureData2.npy'),
np.load('MeasureData4.npy'), np.load('MeasureData5.npy'),
np.load('MeasureData6.npy')))
AllMeasuredData = np.vstack(
(np.load('MeasureData1.npy'), np.load('MeasureData2.npy'),
np.load('MeasureData3.npy'), np.load('MeasureData4.npy'),
np.load('MeasureData5.npy'), np.load('MeasureData6.npy')))
f = km.KalmanFilter(dim_x=Net.X_Vector.size,
dim_z=Net.MeasurementData[:, i].size)
#f = km.SquareRootKalmanFilter(dim_x = Net.X_Vector.size, dim_z = Net.MeasurementData[:,i].size)
f.x = Net.X_Vector.T
f.F = Net.A_Matrix.todense()
f.H = Net.H_Matrix
f.P = Net.P_Matrix.todense()
f.R = Net.R_Matrix
f.Q = Net.Q_Matrix.todense()
#######
## Iterating
#TI.kalman_iteration(Net,Iterations)
#TI.forward_Prediction(Net,Iterations)
P_negative = np.zeros((tmax, nx, nx)) P_positive = np.zeros((tmax, nx, nx)) x_true[:, 0] = np.array([1, 1, 0, 1.5]) x_estimate[:, 0] = np.array(x_true[:, 0] - 1) P_positive[0] = np.eye(nx) * 1 P_negative[0] = np.eye(nx) * 1 sigmav = np.diag([1, 1]) * 30 sigmaW = np.array([1, 1]) * 2 R = sigmav**2 Q = np.diag(sigmaW**2) Q2 = np.diag([1, 1, 1]) t = np.linspace(0, tmax * T, tmax) constant_velocity = kfilter.KalmanFilter(dim_x=4, dim_z=2) constant_velocity.F = F constant_velocity.H = H constant_velocity.Q = Gamma.dot(Q).dot(Gamma.T) constant_velocity.R = R constant_velocity.x = x_true[:, 0] constant_velocity.P = P_positive[0] constant_acceleration = kfilter.KalmanFilter(dim_x=6, dim_z=2) constant_acceleration.F = F_cv constant_acceleration.H = H2 constant_acceleration.Q = Gamma_ca.dot(Q).dot(Gamma_ca.T) constant_acceleration.R = R constant_acceleration.x = np.array([1, 0, 0, 0, 0, 0]) * 1.0 constant_acceleration.P = np.diag([2**2, 2**2] * 2 + [2**2] * 2)
def __init__(
self,
robot: Any,
use_sensor_interface: bool = True,
accelerometer_variance=0.1,
observation_variance=0.1,
initial_variance=0.1,
velocity_filter_window: float = _DEFAULT_VELOCITY_FILTER_WINDOW,
gyroscope_filter_window: float = _DEFAULT_GYRO_FILTER_WINDOW,
contact_detection_threshold: float = 0.0,
velocity_clipping: float = _DEFAULT_VELOCITY_CLIPPING,
):
"""Initializes the class.
Args:
robot: A quadruped robot.
use_sensor_interface: Whether to use the sensor interface to obtain the
IMU readings or directly get them from the robot class. Former should
be used in simulation to enable added latency and noise while latter
should be used on real robot for better performance.
accelerometer_variance: The estimated variance in the accelerometer
readings, used in the Kalman Filter.
observation_variance: The estimated variance in the observed CoM velocity
from the stance feet velocities, used in the Kalman Filter.
initial_variance: The variance of the initial distribution for the
estimated CoM variance.
velocity_filter_window: The filtering window (in time) used to smooth the
estimated CoM velocity.
gyroscope_filter_window: The filtering window (in time) used to smooth the
input gyroscope readings.
contact_detection_threshold: Threshold on the contact sensor readings to
determine whether the foot is in contact with the ground.
velocity_clipping: Clipping value for the estimated velocity to prevent
unrealistically large velocity estimations.
"""
self._robot = robot
self._contact_detection_threshold = contact_detection_threshold
self._velocity_clipping = velocity_clipping
self._use_sensor_interface = use_sensor_interface
# Use the accelerometer and gyroscope sensor from the robot
if self._use_sensor_interface:
for sensor in self._robot.sensors:
if isinstance(sensor,
accelerometer_sensor.AccelerometerSensor):
self._accelerometer = sensor
if isinstance(sensor, imu_sensor.IMUSensor):
self._gyroscope = sensor
assert hasattr(
self, "_accelerometer") and self._accelerometer is not None
assert hasattr(self, "_gyroscope") and self._gyroscope is not None
# x is the underlying CoM velocity we want to estimate, z is the observed
# CoM velocity from the stance feet velocities, and u is the accelerometer
# readings.
self._filter = kalman.KalmanFilter(dim_x=_STATE_DIMENSION,
dim_z=_STATE_DIMENSION,
dim_u=_STATE_DIMENSION)
# Initialize the state distribution to be a zero-mean multi-variate normal
# distribution with initial variance.
self._filter.x = np.zeros(_STATE_DIMENSION)
self._initial_variance = initial_variance
self._filter.P = np.eye(_STATE_DIMENSION) * self._initial_variance
# Covariance matrix for the control variable.
self._filter.Q = np.eye(_STATE_DIMENSION) * accelerometer_variance
# Covariance matrix for the observed states.
self._filter.R = np.eye(_STATE_DIMENSION) * observation_variance
# observation function (z=H*x+N(0,R))
self._filter.H = np.eye(_STATE_DIMENSION)
# state transition matrix (x'=F*x+B*u+N(0,Q))
self._filter.F = np.eye(_STATE_DIMENSION)
# Control transition matrix
self._filter.B = np.eye(_STATE_DIMENSION)
self._velocity_filter = time_based_moving_window_filter.TimeBasedMovingWindowFilter(
velocity_filter_window)
self._gyroscope_filter = time_based_moving_window_filter.TimeBasedMovingWindowFilter(
gyroscope_filter_window)
self.reset(0)
def multivariate_kalman_old(reader_data, fingerprint):
"""
This function will be responsible for finding the trajectory of the
reader given the reader's returned rssi values and the matrix containing the
area's default values.
**Parameters**
reader_data:1xn list
This is the list containing the RSSI values sent from the reader as
it moved around the grid.
fingerprint: 7x7 list
This is the matrix which contains standard RSSI values for each point in
the grid. Each point should be a single value and is assumed to have
been calculated using the fingerprint_average() or univariate_kalman()
functions.
"""
# Calculate the sample standard deviation of the rssi_data so for the
# measurement noise value.
standard_deviation = statistics.stdev(reader_data)
# This Kalman filter object will track the position in two dimensions using
# only the inputted RSSI value.
# The state variable will also store the two previous x and y values for use
# in the state transition matrix.
reader = kalman.KalmanFilter(dim_x=4, dim_z=1)
# The position (state variable) is stored as cartesian coordinates and
# initialised to the centre of the grid.
# Feature space for this step is in terms of grid coordinates.
# The conversion between measurement space and coordinate space is carried
# out below.
# List element values are: [x_0, y_0, x_-1, y_-1].
# Talk about hackiness of adding another time period into measurements!!!!!
reader.x = np.array([[3, 3, 3, 3]])
"""
The state transition matrix describes the calculation which needs to be
carried out on the state variable in order to obtain the predicted state
variables.
In the book example, the state variables are the position (x) and the
velocity (x dot) of the dog. The state transition matrix then describes the
equations:
x- = (x.)(dt) + x
x. = x.
The acceleration is constant as it is assumed to be a non-factor in this
scenario in order to keep it simple.
In our case, the matrix must calculate in which direction the reader is
going and extrapolate from there. For simplicity's sake, the previous
direction of travel could be taken as the foundation for the prediction.
"""
# State transition matrix - "transformation of the state matrix" must be
# carried out.
# This matrix first adds the difference between the current and previous
# values for x and y, then assigns the 'current' values for x and y to the
# 'previous' position in the output vector.
reader.F = np.array([[2, 0, -1, 0],
[0, 2, 0, -1],
[1, 0, 0, 0],
[0, 1, 0, 0]])
# Motion function not needed ('Bu' term in book).
"""
In the book example, a measurement function was not needed as the
residual was in the same units as the measured quantity - the entire process
took place in the same feature space (distance). In this case, the feature
space is the measurement space, so RSSI. However, we cannot convert from
position to RSSI just as we cannot convert from RSSI to position yet -
the process is not deterministic and gains accuracy as the dataset grows
larger.
"""
# Measurement function matrix. This is used to calculate the residual.
# This is used by the object to convert from coordinates → RSSI
reader.H = np.array()
# Covariance/state variance matrix - this will always be of dimensions n^2
# where n is the number of state variables. As we have no idea where the
# reader starts and x and y are independent, they will be 0. Therefore, the
# identity matrix to which it is initialized to can be multiplied
# element-wise by a large number to indicate a large uncertainty.
reader.P *= 1000
# Measurement noise.
reader.R = np.array(standard_deviation)
# Process noise.
reader.Q = Q_discrete_white_noise(dim=2, dt=3)
positions = [[0, 0, 0, 0] for i in range(len(reader_data))]
for i, rssi in enumerate(reader_data):
reader.predict()
reader.update(rssi)
positions[i] = reader.x
return positions
def multivariate_kalman(coordinates_matrix): """ :param coordinates_matrix: 2*10 matrix of x-y coordinates from the trajectory :param fingerprint: 7*7 matrix of RSSI values :return: 2*10 matrix of filtered coordinate pairs """ reader = kalman.KalmanFilter(dim_x=4, dim_z=2) # STATE VARIABLE # will be of format [x,y,v_x,v_y] # velocity is the unobserved variable reader.x = np.array([[3, 3, 0, 0]]).T # STATE TRANSITION MATRIX # time difference between measurements is assumed to be 3 seconds reader.F = np.array([[1, 0, 3, 0], [0, 1, 0, 3], [0, 0, 1, 0], [0, 0, 0, 1]]) # MEASUREMENT MATRIX """ The measurement function converts from state variable units. As we do not want to convert units until the very end of the filtering, this will not change x and y. The shape is derived from z = Hx. """ reader.H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # using sample variance x_var = statistics.variance([p[0] for p in coordinates_matrix]) y_var = statistics.variance([p[1] for p in coordinates_matrix]) # MEASUREMENT NOISE # assume no covariance - another naïve assumption reader.R = np.array([[x_var, 0], [0, y_var]]) # PROCESS NOISE - this is a complete guess reader.Q = np.array([[0.5, 0, 0, 0], [0, 0.5, 0, 0], [0, 0, 0.5, 0], [0, 0, 0, 0.5]]) # COVARIANCE MATRIX reader.P = np.array([[500, 0, 0, 0], [0, 500, 0, 0], [0, 0, 500, 0], [0, 0, 0, 500]]) positions = [] for point in coordinates_matrix: reader.predict() reader.update(np.array(point).T) """ reader.x[0] contains the original xy coordinates passed in """ positions.append(reader.x[0]) print(np.array(positions)) return positions
""" This is the first EKF in the text. It diverges for reasons discussed
in the text. It is not a good filter."""
signoise = 1000.
beta = 500
ts = 0.1 # time step, dt in my world
tf = 30. #end time
phis = 0. # spectral noise in Q. set to 100 to avoid divergence in filter
t = 0. # time
s = 0.
h = 0.001
g = 32.2
ekf = kf.KalmanFilter(dim_x=2,dim_z=1)
ekf.F = np.zeros((2,2)) # PHI
ekf.x = np.array([[200025],[-6150.]]) #xh, xdh
ekf.Q = np.zeros((2,2))
ekf.P = np.array([[signoise**2, 0.],
[0, 20000.]])
ekf.H = np.array([[1., 0.]])
pos = (200000., -6000.)
poss = []
fzs = []
fs = []
Measurements['9'] = np.load(Directory +
'Node_Head_9.npy') + MeasurementNoise[9, :]
Measurements['10'] = np.load(Directory +
'Node_Head_10.npy') + MeasurementNoise[10, :]
MeasurementData = []
for i in Measurement_Nodes:
MeasurementData.append(Measurements[i])
MeasurementData = np.array(MeasurementData).T
AllMeasurementData = []
for i in All_Nodes:
AllMeasurementData.append(Measurements[i])
AllMeasurementData = np.array(AllMeasurementData).T
kf = km.KalmanFilter(dim_x=Net.X_Vector.size, dim_z=MeasurementData[1, :].size)
#f = km.SquareRootKalmanFilter(dim_x = Net.X_Vector.size, dim_z = Net.MeasurementData[:,i].size)
kf.x = Net.X_Vector.T
kf.F = Net.A_Matrix.todense()
kf.H = Net.H_Matrix
kf.P = Net.P_Matrix.todense()
kf.R = Net.R_Matrix
kf.Q = Net.Q_Matrix.todense()
#kf.alpha = 1.01
X_kf = np.zeros((Iterations, kf.x.size))
#######
## Iterating
#kf.x = kf.x.T
for i in range(0, Iterations):
