def __init__(self, dt, wheelbase):
EKF.__init__(self, 3, 2, 2)
self.dt = dt
self.wheelbase = wheelbase
a, x, y, v, w, theta, time = symbols(
'a, x, y, v, w, theta, t')
d = v*time
beta = (d/w)*sympy.tan(a)
r = w/sympy.tan(a)
self.fxu = Matrix([[x-r*sympy.sin(theta)+r*sympy.sin(theta+beta)],
[y+r*sympy.cos(theta)-r*sympy.cos(theta+beta)],
[theta+beta]])
self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
self.V_j = self.fxu.jacobian(Matrix([v, a]))
self.subs = {x: 0, y: 0, v:0, a:0, time:dt, w:wheelbase, theta:0}
self.x_x = x
self.x_y = y
self.v = v
self.a = a
self.theta = theta
def __init__(self):
self.wx = 100
self.wy = 0
self.wz = 0
self.q1 = 0
self.q2 = 1
self.q3 = 0
self.q4 = 0
self.rk = ExtendedKalmanFilter(dim_x=7, dim_z=2)
def extend_kf(self, number=240): '''扩展卡尔曼滤波算法, 初步看滤波效果''' global Time P0 = np.diag([ 3e-1, 3e-1, 3e-1, 1e-6, 1e-6, 1e-6, 3e-1, 3e-1, 3e-1, 1e-6, 1e-6, 1e-6 ]) error = np.random.multivariate_normal(mean=np.zeros(12), cov=P0) X0 = np.hstack( (HPOP_1[0], HPOP_2[0]) ) + error X, P, I = [X0], [P0], identity(12) ekf = EKF(dix_x=12, dim_z=4, compute_log_likelihood=False) ekf.x = X0; ekf.F = self.jacobian_double(Time, )
def __init__(self):
self.rk = ExtendedKalmanFilter(dim_x=7, dim_z=2)
self.wx = 0
self.wy = 0
self.wz = 265
self.q1 = 1
self.q2 = 0
self.q3 = 0
self.q4 = 0
#range_std = 5000
self.rk.R = np.diag([300,500])
self.rk.Q = np.diag([50,50,50,50,0.1,0.1,0.1])
self.rk.P *= 50
self.rk.x = np.array([[self.q1,self.q2,self.q3,self.q4,self.wx,self.wy,self.wz]]).T
def __init__(self, dt, wheelbase):
EKF.__init__(self, 3, 2, 2)
rospy.init_node('ekf', anonymous=True)
#self.sub = rospy.Subscriber('/odom', Odometry, self.getOdom)
self.pub = rospy.Publisher('/odom_ekf', Odometry, queue_size=0)
self.tf_br = tf.TransformBroadcaster()
self.dt = dt
self.wheelbase = wheelbase
self.OdomFiltered = Odometry()
self.m = array([[0, 0]])
self.u = array([.0, .0])
self.PP = np.mat(np.diag([0.0] * 3))
rospy.wait_for_message("/odom", Odometry)
rospy.sleep(1)
def __init__(self, starting_value):
self._filter = ExtendedKalmanFilter(dim_x=2, dim_z=1)
self._filter.x = np.array([starting_value, starting_value])
self._filter.F = np.array([[1.,1.],
[0.,1.]])
self._filter.H = np.array([[1.,0.]])
self._filter.P = np.array([[1000., 0.],
[ 0., 1000.] ])
self._filter.R = np.array([[5.]])
self._filter.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
self.inc = 0
self.last_val = starting_value
self.first = True
def get_rk():
global rk
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
dt = 0.01
rk.F = asarray([[1., dt, dt * dt / 2],
[0., 1., dt],
[0., 0., 1.],
])
# 初始位置 先设置为0
rk.x = array([0, 0, 0]).T
# 测量误差
rk.R *= 0.001
rk.P *= 0.1
rk.Q *= 0.001
return rk
def predict(self, u=0):
'''
Prediction step of the estimator.
Parameters
----------
u: float
Input of the system.
Notes
-----
This is a correction of the behavior of IMM and ExtendedKalman Filter not
working correctly together.
'''
if u is None:
u = 0
ExtendedKalmanFilter.predict(self, u=u)
def __init__(self, path):
dt = 0.05
self.rk = ExtendedKalmanFilter(dim_x=2, dim_z=1)
# radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
#self.detector = PlanetDetector('C:\\Users\\Phuc Dang\\Desktop\\Developer\\Collaborative-Sensing\\model\\cascade.xml')
self.detector = PlanetDetector(path)
# make an imperfect starting guess
# rk.x = array([radar.pos - 100, radar.vel + 100, radar.alt + 1000])
self.rk.x = array([0, .273]) # need better initial location
self.rk.F = eye(2) + array([[0, 0], [1, 0]]) * dt
range_std = 5. # meters
self.rk.R = np.diag([range_std**2])
self.rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
self.rk.Q[1, 1] = 0.1
self.rk.P *= 50
def __init__(self, dt, wheelbase, std_vel, std_steer):
EKF.__init__(self, 3, 2, 2)
self.dt = dt
self.wheelbase = wheelbase
self.std_vel = std_vel
self.std_steer = std_steer
a, x, y, v, w, theta, time = symbols('a, x, y, v, w, theta, t')
d = v * time
beta = (d / w) * sympy.tan(a)
r = w / sympy.tan(a)
# save dictionary and it's variables for later use
self.subs = {x: 0, y: 0, v: 0, a: 0, time: dt, w: wheelbase, theta: 0}
self.x_x, self.x_y, = x, y
self.v, self.a, self.theta = v, a, theta
def get_rk():
global rk
global rotating_model_jacobian
rotating_model_jacobian = Rotating_Model_Jacobian()
rk = ExtendedKalmanFilter(dim_x=7, dim_z=3)
dt = 0.01
rk.F = asarray([[1., 0., 0., 0., 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., 0., 0., 0., 1., dt, dt * dt / 2],
[0., 0., 0., 0., 0., 1., dt], [0., 0., 0., 0., 0., 0.,
1.]])
# 初始位置 先设置为0
rk.x = array([1, 1, 1, 0.4, 0, 1, 1]).T
# 测量误差
rk.R *= 0.001
rk.P *= 1
rk.Q *= 0.001
return rk
def __init__(self, dim_x, dim_z, s0, q0, points0, w0, v0, dim_u=0):
ExtendedKalmanFilter.__init__(self, dim_x, dim_z, dim_u)
# 初始状态估计
self.x[0][0] = s0[0] / s0[2]
self.x[1][0] = s0[1] / s0[2]
self.x[2][0] = v0[0] / s0[2]
self.x[3][0] = v0[1] / s0[2]
self.x[4][0] = v0[2] / s0[2]
self.x[5][0] = q0[0]
self.x[6][0] = q0[1]
self.x[7][0] = q0[2]
self.x[8][0] = q0[3]
self.x[9][0] = w0[0]
self.x[10][0] = w0[1]
self.x[11][0] = w0[2]
for i in range(0, M):
self.x[3 * i + 12][0] = points0[i][0] / s0[2]
self.x[3 * i + 13][0] = points0[i][1] / s0[2]
self.x[3 * i + 14][0] = points0[i][2] / s0[2]
def __init__(self, K, dim_x=3, dim_z=3, lamb=2, sigma=2, dt=0.05):
#Init Kalman filter
self.rk = ExtendedKalmanFilter(dim_x=dim_x, dim_z=dim_z)
self.rk.x = np.array([1, 1, 1])
self.sigma = sigma
self.lamb = lamb
self.K = K
self.rho = 28.0
self.sigma_lorenz = 10.0
self.beta = 8.0 / 3.0
self.dt = dt
self.rk.F = self.get_F(self.rk.x)
self.rk.R = np.diag([lamb**2]*3)
self.rk.Q = self.get_Q(dt=self.dt)
self.rk.H = np.identity(3, dtype=np.float32)
self.rk.P *= 3
def __init__(self, dt, std_vel, std_steer):
EKF.__init__(self, 3, 2, 2)
self.dt = dt
self.std_vel = std_vel
self.std_steer = std_steer
beta, x, y, v, theta, time = symbols('beta, x, y, v, theta, t')
d = v * time
self.fxu = Matrix([[x + d * sympy.sin(theta + beta)],
[y + d * sympy.cos(theta + beta)], [theta + beta]])
self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
self.V_j = self.fxu.jacobian(Matrix([v, beta]))
# save dictionary and its variables for later use
self.subs = {x: 0, y: 0, v: 0, beta: 0, time: dt, theta: 0}
self.x_x, self.x_y, = x, y
self.v, self.beta, self.theta = v, beta, theta
def test_initialization_wrong_gamma(self):
with self.assertRaises(ValueError):
filter = ExtendedKalmanFilter(dim_x = 9, dim_z = 6)
gamma = np.array([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
attacker = PeriodAttacker(filter = self.filter,
radar = self.radar, radar_pos = self.radar_pos,
gamma = gamma,mag_vector = self.mag_vector,
t0 = self.t0, time = self.time)
def test_saver_ekf():
def HJ(x):
return np.array([[1, 1]])
def hx(x):
return np.array([x[0]])
kf = ExtendedKalmanFilter(2, 1)
s = Saver(kf)
for i in range(3):
kf.predict()
kf.update([[i]], HJ, hx)
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 3
assert len(s.y) == 3
assert len(s.K) == 3
def get_rk(self):
self.rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
dt = 0.01
self.rk.F = asarray([[1., dt, dt * dt / 2], [0., 1., dt], [0., 0.,
1.]])
# 初始位置 先设置为0
# self.rk.x = array([0, 0, 0.2]).T
self.rk.x = self.initialState
# 测量误差
self.rk.R *= 0.01
self.rk.P *= 10
self.rk.Q *= 0.001
return self.rk
def __init__(self, dt, wheelbase, std_acc, std_steer):
EKF.__init__(self, 5, 2, 2)
self.dt = dt
self.wheelbase = wheelbase
self.std_acc = std_acc
self.std_steer = std_steer
alpha, a, x, y, v, w, theta, dtheta, time = symbols(
'alpha, a, x, y, v, w, theta, dtheta, t')
d = v
# + a/2*time**2
beta = (d / w) * sympy.tan(alpha)
r = w / sympy.tan(alpha)
self.fxu = Matrix(
[[x - r * sympy.sin(theta) + r * sympy.sin(theta + dtheta * time)],
[y + r * sympy.cos(theta) - r * sympy.cos(theta + dtheta * time)],
[v + a * time], [theta + dtheta * time], [beta]])
self.F_x = self.fxu.jacobian(Matrix([x, y, v, theta, dtheta]))
self.F_u = self.fxu.jacobian(Matrix([a, alpha]))
print('F_x', sympy.simplify(self.F_x))
print('F_u', sympy.simplify(self.F_u))
self.subs = {
x: 0,
y: 0,
v: 0,
alpha: 0,
a: 0,
time: dt,
w: wheelbase,
theta: 0
}
self.x_x, self.x_y = x, y
self.v, self.alpha, self.theta, self.dtheta, self.a = v, alpha, theta, dtheta, a
def setUp(self):
# Simulated filter for 2 radars (2*3 measurements)
self.filter = ExtendedKalmanFilter(dim_x = 9, dim_z = 6)
self.gamma = np.eye(3)
self.mag_vector = np.array([[-10, -10, -10]]).T
self.t0 = 10
self.time = 50
self.radar = Radar(x=10,y=10)
self.radar_pos = 1
self.attacker = PeriodAttacker(filter = self.filter,
radar = self.radar, radar_pos = self.radar_pos,
gamma = self.gamma,mag_vector = self.mag_vector,
t0 = self.t0, time = self.time)
class EKF:
def __init__(self, path):
dt = 0.05
self.rk = ExtendedKalmanFilter(dim_x=2, dim_z=1)
# radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
#self.detector = PlanetDetector('C:\\Users\\Phuc Dang\\Desktop\\Developer\\Collaborative-Sensing\\model\\cascade.xml')
self.detector = PlanetDetector(path)
# make an imperfect starting guess
# rk.x = array([radar.pos - 100, radar.vel + 100, radar.alt + 1000])
self.rk.x = array([0, .273]) # need better initial location
self.rk.F = eye(2) + array([[0, 0], [1, 0]]) * dt
range_std = 5. # meters
self.rk.R = np.diag([range_std**2])
self.rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
self.rk.Q[1, 1] = 0.1
self.rk.P *= 50
def HJacobian_at(x):
""" compute Jacobian of H matrix at x """
angular_vel = x[1]
angular_pos = x[0]
return array([[0., (-1) * np.sin(angular_pos) * constant]])
def hx(x):
""" compute measurement for slant range that
would correspond to state x.
"""
return constant * cos(x[0])
def run_filter(self):
while 1:
self.detector.runCascadeClassifier()
z = self.detector.get_last_measurement()
self.rk.update(array([z]), self.HJacobian_at, self.hx)
self.rk.predict()
print(self.rk.x)
def __init__(self, dt, wheelbase, sigma_vel, sigma_steer):
EKF.__init__(self, 3, 2, 2)
self.dt = dt
self.wheelbase = wheelbase
self.sigma_vel = sigma_vel
self.sigma_steer = sigma_steer
a, x, y, v, w, theta, time = symbols('a, x, y, v, w, theta, t')
d = v * time
beta = (d / w) * sympy.tan(a)
r = w / sympy.tan(a)
self.fxu = Matrix(
[[x - r * sympy.sin(theta) + r * sympy.sin(theta + beta)],
[y + r * sympy.cos(theta) - r * sympy.cos(theta + beta)],
[theta + beta]])
self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
self.V_j = self.fxu.jacobian(Matrix([v, a]))
# save dictionary and it's variables for later use
self.subs = {x: 0, y: 0, v: 0, a: 0, time: dt, w: wheelbase, theta: 0}
self.x_x, self.x_y, self.v, self.a, self.theta = x, y, v, a, theta
def __init__(self, planetDetector, FIFO_FILENAME, OTHER_FIFO):
# Haar classifier
self.detector = planetDetector
# EKF model
self.rk = ExtendedKalmanFilter(dim_x=2, dim_z=1)
self.initialize_rk()
# FIFO
self.writeFiFo = os.open(FIFO_FILENAME, os.O_WRONLY)
self.readFiFo = os.open(OTHER_FIFO, os.O_RDONLY)
if FIFO_FILENAME == "srv_transmit":
self.server = True
else:
self.server = False
#Merge values
self.omega = self.rk.x[0]
self.omega_hat = self.rk.x[1]
#Client sensor values
self.omega_client = self.rk.x[0]
self.omega_hat_client = self.rk.x[1]
def __init__(self, wheelbase, std_vel, std_steer):
ExtendedKalmanFilter.__init__(self, 3, 2, 2)
self.wheelbase = wheelbase
self.std_vel = std_vel # standard deviation of velocity
self.std_steer = std_steer # standard deviation of steering
# 定义符号
a, x, y, v, w, theta, time = symbols(
'a, x, y, v, w, theta, t')
d = v * time # km
beta = (d / w) * sympy.tan(a) # rad
r = w / sympy.tan(a) # km
# FV影响P
# r_lo = r / 111 / sympy.cos(y / 180 * np.pi) # km-->longitude
# r_la = r / 111 # km-->latitude
# # 定义predict转移函数h形式，单位
# self.fxu = Matrix(
# [[x - r_lo * sympy.sin(theta) + r_lo * sympy.sin(theta + beta)],
# [y + r_la * sympy.cos(theta) - r_la * sympy.cos(theta + beta)],
# [theta + beta]])
# 定义predict转移函数h形式，单位
self.fxu = Matrix(
[[x - r * sympy.sin(theta) + r * sympy.sin(theta + beta)],
[y + r * sympy.cos(theta) - r * sympy.cos(theta + beta)],
[theta + beta]])
# 定义转移矩阵H和控制矩阵V（对转移函数h求偏导）
self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
self.V_j = self.fxu.jacobian(Matrix([v, a]))
# 定义变量字典save dictionary and it's variables for later use
self.subs = {x: 0, y: 0, v: 0, a: 0,
time: 1/64, w: wheelbase, theta: 0}
self.x_x, self.x_y, = x, y
self.v, self.a, self.theta = v, a, theta
def __init__(self):
if (VELOCITY):
self.f = ExtendedKalmanFilter(dim_x=4,
dim_z=(len(ANCHORS) - 1 + 2))
# The estimate
self.f.x = np.array([[1.], [1.], [0.], [0.]])
# State transition matrix, just use last position for now
self.f.F = np.array([[1., 0., 0.12, 0.], [0., 1., 0., 0.12],
[0., 0., 1., 0.], [0., 0., 0., 1.]])
# Measurement function, values from linear localization
self.f.P = np.eye(
4
) * P # Covaraince, large number since our initial guess is (1, 1)
self.f.R = np.eye(
len(ANCHORS) - 1 + 2
) * R # Measurement uncertainty, making it high since noisy data
self.f.Q = np.eye(
4
) * Q # System noise, making it small since we're using a velocity estimation
else:
self.f = ExtendedKalmanFilter(dim_x=2, dim_z=len(ANCHORS) - 1)
# The estimate
self.f.x = np.array([[1.], [1.]])
# State transition matrix, just use last position for now
self.f.F = np.array([[1., 0.], [0., 1.]])
# Measurement function, values from linear localization
self.f.P = np.eye(
2
) * P # Covaraince, large number since our initial guess is (1, 1)
self.f.R = np.eye(
len(ANCHORS) - 1
) * R # Measurement uncertainty, making it low since we want to weight sensor data
self.f.Q = np.eye(
2
) * Q # System noise, making it big since we're just using the last location as our prediction
def initFilter(app):
app.f = ExtendedKalmanFilter(dim_x=2, dim_z=len(ANCHORS) - 1)
# The estimate
app.f.x = np.array([[1.], [1.]])
# State transition matrix, just use last position for now
app.f.F = np.array([[1., 0.], [0., 1.]])
# Measurement function, values from linear localization
app.f.P = np.eye(
2) * 1000 # Covaraince, large number since our initial guess is (1, 1)
app.f.R = np.eye(
len(ANCHORS) - 1
) * 50 # Measurement uncertainty, making it low since we want to weight sensor data
app.f.Q = np.eye(
2
) * 1 # System noise, making it big since we're just using the last location as our prediction
def main(): gps_n = Semaphore(0) gps_s = Semaphore(1) gps_coords_stack = Manager().list() gps = GPS(gps_coords_stack, gps_n, gps_s) gps.start() # Get the first position z = gps.getPosition() dt = 0.05 range_std = 5. # Means meters # Instantiate the filter filterk = ExtendedKalmanFilter(2, 1, 0) # 1 type of value of position, but in 2 dimensions. sensor provides position in (x,y) so use 2 # Insert first position filterk.x = array(z) # Pretty sure this sets up the taylor series filterk.F = eye(2) + array([[0,1], [0,0]])*dt # Sets the uncertainty filterk.R = np.diag([range_std**2]) # Trains it using white noise? filterk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1) filterk.Q[2, 2] = 0.1 # Covariance matrix filterk.P *= 50 for i in range(10): # Pull a value from the GPS stack gps_n.acquire() gps_s.acquire() result = gps_coords_stack.pop() gps_s.release() # Put new z value in filterk.predict_update(array(result), HJacobian_at, hx) #this maaaaay need to be formatted differently, otherwise just put the longitude and lattitude as an array [x,y] # Get the predicted value np.append(xs, filterk.x) print(filterk.x)
def setUp(self):
# Simulated filter for 2 radars (2*3 measurements)
self.filter = ExtendedKalmanFilter(dim_x=9, dim_z=6)
# Attack matrix: second radar is compromised
self.gamma = np.array([[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]])
self.mag_vector = np.array([[0, 0, 0, -10, -10, -10]]).T
self.t0 = 10
self.time = 50
self.radar = Radar(x=10, y=10)
self.attacker = Attacker(filter=self.filter,
radar=self.radar,
gamma=self.gamma,
mag_vector=self.mag_vector,
t0=self.t0,
time=self.time)
class FusionEKF:
def __init__(self):
self.dt = 0.5
self.proccess_error = 0.05
self.ekf = EKF(3, 3)
self.ekf.Q = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
def run(self, data):
true_position = []
for i in range(int(20 / self.dt)):
z = data
self.ekf.update(np.asarray([z.lidar.data]),
self.H_of,
self.hx,
R=self.hx(self.ekf.x) * self.proccess_error)
self.ekf.predict()
self.ekf.update(np.asarray([z.camera.data]),
self.H_of,
self.hx,
R=self.hx(self.ekf.x) * self.proccess_error)
self.ekf.predict()
true_position.append(self.ekf.x)
def H_of(self, x):
return 0
def hx(self, x):
return 0
def fx(self, x):
return np.dot(self.efk.F, x)
def setup():
# set up sensor simulator
F = np.array([[1, dt, 0], [0, 1, 0], [0, 0, 1]])
def f(x):
return np.dot(F, x)
x0 = np.array([[-5], [0.5], [1]])
sim = Sensor(x0, f, h)
# set up kalman filter
tracker = ExtendedKalmanFilter(dim_x=3, dim_z=2)
tracker.F = F
q_x = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
q_y = 0.0001 * dt**2
tracker.Q = block_diag(q_x, q_y)
tracker.R = R_proto * measurement_var_max * filter_misestimation_factor
tracker.x = np.array([[-5, 0.5, 1]]).T
tracker.P = np.eye(3) * 500
return sim, tracker
def test_saver_ekf():
def HJ(x):
return np.array([[1, 1]])
def hx(x):
return np.array([x[0]])
kf = ExtendedKalmanFilter(2, 1)
s = Saver(kf)
for i in range(3):
kf.predict()
kf.update([[i]], HJ, hx)
s.save()
# this will assert if the KalmanFilter did not properly assert
s.to_array()
assert len(s.x) == 3
assert len(s.y) == 3
assert len(s.K) == 3
def build_ekf(coeffs, z_data, linear_consts=None, nmsmt=n_msmt, dx=dimx):
global n_msmt
global dimx
(dimx, n_msmt) = (dx, nmsmt)
ekf = ExtendedKalmanFilter(dim_x=dimx, dim_z=n_msmt)
#ekf.x = zeros([dimx, 1])
#ekf.__init__(dimx, n_msmt)
if len(coeffs):
coeffs = array(coeffs).flatten()
if n_coeff == dimx * 2 + n_msmt:
ekf.Q = symmetric(array(coeffs[0:dimx]))
ekf.F = symmetric(array(coeffs[dimx:dimx * 2]))
r = symmetric(array(coeffs[-n_msmt:]))
else:
ekf.Q = read2d(coeffs, dimx, 0, dimx * dimx)
ekf.F = read2d(coeffs, dimx, dimx * dimx, dimx * dimx * 2)
r = read2d(coeffs, n_msmt, -n_msmt * n_msmt, n_coeff)
logger.info("ekf.Q=" + str(ekf.Q) + ", ekf.F = " + str(ekf.F) +
", r = " + str(r))
return update_ekf(ekf, z_data, r, linear_consts)
return update_ekf(ekf, z_data)
# Initialize measurement measure = Measure(debug_mode=False) # Initialize flow detection flow = Flow() sock = udpstuff.init() # Initialize Kalman # Initialize Extended Kalman Filter rk = ExtendedKalmanFilter(dim_x=3, dim_z=1) # Starting guess location rk.x = array([[args.x, args.y, args.theta]]).T # Motion Noise rk.Q = np.diag([range_std**2, range_std**2, math.radians(range_angle)**2]) # Measurement Noise rk.R = np.array([[math.radians(april_angle)**2]]) # Covariance matrix rk.P = np.diag([XCOV**2, YCOV**2, math.radians(THCOV)**2])
def __init__(self, dt):
EKF.__init__(self, 3, 2, 2)
self.dt = dt
def __init__(self, dt, wheelbase):
EKF.__init__(self, 3, 2, 2)
self.dt = dt
self.wheelbase = wheelbase
jacobian.append ([diff_x/denom, diff_y/denom])
return array(jacobian)
def hx (x):
ranges = []
est_x = x.item (0)
est_y = x.item (1)
for s in radar.sensors:
ranges.append ([sqrt((est_x - s[0])**2 + (est_y - s[1])**2)])
return ranges
radar = RadarNet ()
ekf = ExtendedKalmanFilter (dim_x = 2, dim_z = 3)
ekf.x = array([[1.0], [1.0]])
# ekf.x = array([[radar.pos, radar.vel+100, radar.alt+1000]]).T
# ekf.x = array([[0.0, 0.0, 0.0]]).T
ekf.F = eye(2)
# ekf.R = radar.target[1] * 0.05
ekf.R *= 10
ekf.Q *= 0.0001
ekf.P *= 50
truth_x = []
truth_y = []
estimated_x = []
#
d0 = 10
def HJacobian_at(x):
angular_vel = x[1]
angular_pos = x[0]
return array([[0., (-1)*np.sin(angular_pos)*d0]])
def get_pixel_between_sun_and_planet(x):
return d0 * cos(x[0])
dt = 0.05
rk = ExtendedKalmanFilter(dim_x=2, dim_z=1)
#radar = RadarSim(dt, pos=0., vel=100., alt=1000.)
detector = PlanetDetector('../old_models/model_4/cascade.xml', False, False)
# make an imperfect starting guess
#rk.x = array([radar.pos - 100, radar.vel + 100, radar.alt + 1000])
rk.x = array([0, 0.237]) #need better initial location
rk.F = eye(2) + array([[0, 0],
[1, 0]]) * dt
range_std = 5. # meters
bs = np.diag([range_std ** 2])
print(bs)
def test_ekf():
def H_of(x):
""" compute Jacobian of H matrix for state x """
horiz_dist = x[0]
altitude = x[2]
denom = sqrt(horiz_dist**2 + altitude**2)
return array([[horiz_dist/denom, 0., altitude/denom]])
def hx(x):
""" takes a state variable and returns the measurement that would
correspond to that state.
"""
return sqrt(x[0]**2 + x[2]**2)
dt = 0.05
proccess_error = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
rk.F = eye(3) + array ([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])*dt
def fx(x, dt):
return np.dot(rk.F, x)
rk.x = array([-10., 90., 1100.])
rk.R *= 10
rk.Q = array([[0, 0, 0],
[0, 1, 0],
[0, 0, 1]]) * 0.001
rk.P *= 50
rs = []
xs = []
radar = RadarSim(dt)
ps = []
pos = []
s = Saver(rk)
for i in range(int(20/dt)):
z = radar.get_range(proccess_error)
pos.append(radar.pos)
rk.update(asarray([z]), H_of, hx, R=hx(rk.x)*proccess_error)
ps.append(rk.P)
rk.predict()
xs.append(rk.x)
rs.append(z)
s.save()
# test mahalanobis
a = np.zeros(rk.y.shape)
maha = scipy_mahalanobis(a, rk.y, rk.SI)
assert rk.mahalanobis == approx(maha)
s.to_array()
xs = asarray(xs)
ps = asarray(ps)
rs = asarray(rs)
p_pos = ps[:, 0, 0]
p_vel = ps[:, 1, 1]
p_alt = ps[:, 2, 2]
pos = asarray(pos)
if DO_PLOT:
plt.subplot(311)
plt.plot(xs[:, 0])
plt.ylabel('position')
plt.subplot(312)
plt.plot(xs[:, 1])
plt.ylabel('velocity')
plt.subplot(313)
#plt.plot(xs[:,2])
#plt.ylabel('altitude')
plt.plot(p_pos)
plt.plot(-p_pos)
plt.plot(xs[:, 0] - pos)
# dh_dalt = altitude/denom
return array ([[horiz_dist/denom, 0., altitude/denom]])
def hx(x):
""" takes a state variable and returns the measurement that would
correspond to that state.
"""
return sqrt(x[0]**2 + x[2]**2)
dt = 0.05
proccess_error = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
rk.F = eye(3) + array ([[0, 1, 0],
[0, 0, 0],
[0, 0, 0]])*dt
def fx(x, dt):
return np.dot(rk.F, x)
rk.x = array([-10., 90., 1100.])
rk.R *= 10
def __init__(self, dt):
EKF.__init__(self, 3, 2, 2)
self.dt = dt