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):