def state_estimation():
global dt_v_enc
global v_meas, psi_meas
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('imu/data', Imu, imu_callback)
rospy.Subscriber('encoder', Encoder, enc_callback)
rospy.Subscriber('ecu', ECU, ecu_callback)
state_pub = rospy.Publisher('state_estimate', Z_KinBkMdl, queue_size = 10)
# get vehicle dimension parameters
L_a = rospy.get_param("L_a") # distance from CoG to front axel
L_b = rospy.get_param("L_b") # distance from CoG to rear axel
vhMdl = (L_a, L_b)
# get encoder parameters
dt_v_enc = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x from encoders
# get EKF observer properties
q_std = rospy.get_param("state_estimation/q_std") # std of process noise
r_std = rospy.get_param("state_estimation/r_std") # std of measurementnoise
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
# estimation variables for Luemberger observer
z_EKF = zeros(4)
# estimation variables for EKF
P = eye(4) # initial dynamics coveriance matrix
Q = (q_std**2)*eye(4) # process noise coveriance matrix
R = (r_std**2)*eye(2) # measurement noise coveriance matrix
while not rospy.is_shutdown():
# publish state estimate
(x, y, psi, v) = z_EKF
# publish information
state_pub.publish( Z_KinBkMdl(x, y, psi, v) )
# collect measurements, inputs, system properties
# collect inputs
y = array([psi_meas, v_meas])
u = array([ d_f, acc ])
args = (u,vhMdl,dt)
# apply EKF and get each state estimate
(z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args )
# wait
rate.sleep()
def task(prev_time, error_cov, count, x):
x2 = window.winfo_rootx() + main_window.canvas.winfo_x()
y2 = window.winfo_rooty() + main_window.canvas.winfo_y()
x1 = x2 + main_window.canvas.winfo_width() + 170
y1 = y2 + main_window.canvas.winfo_height() + 150
N = 1000
X = np.linspace(x1, x2, N)
Y = np.linspace(y1, y2, N)
X, Y = np.meshgrid(X, Y)
# Pack X and Y into a single 3-dimensional array
pos = np.empty(X.shape + (4, ))
pos[:, :, 0] = X
pos[:, :, 1] = Y
img = ImageGrab.grab((x2, y2, x1, y1)).save("test_kalman.png")
frame = cv2.imread("test_kalman.png")
# It converts the BGR color space of image to HSV color space
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# preparing the mask to overlay
mask_blue = cv2.inRange(hsv, lower_blue, upper_blue)
mask_green = cv2.inRange(hsv, lower_green, upper_green)
mask_yellow = cv2.inRange(hsv, lower_yellow, upper_yellow)
# The black region in the mask has the value of 0,
# so when multiplied with original image removes all non-blue regions
result = cv2.bitwise_or(cv2.bitwise_or(mask_blue, mask_green), mask_yellow)
masks = [mask_blue, mask_green, mask_yellow]
colors = ["Blue", "Green", "Yellow"]
for i in range(len(masks)):
M = cv2.moments(masks[i])
if M["m00"] != 0:
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
if i == 0:
meas.append((cX, cY))
mp = [cX, cY]
filterstep = time.time() - prev_time
prev_time = time.time()
x, error_cov = ekf(mp, x, filterstep, error_cov, count)
pred.append((int(x[0]), int(x[1])))
paint()
F = multivariate_normal(np.array([int(x[0]),
int(x[1]), 0, 0]), error_cov)
Z = F.pdf(pos)
plt.contourf(X, Y, Z, cmap=cm.viridis)
plt.show(block=False)
cv2.imshow("kalman", frame_kalman)
count += 1
window.after(10, task, prev_time, error_cov, count, x) # reschedule event
def ekf_test(dt,tf,mux0,P0,YK,Qk,Rk):
#Qk = np.array([[1.0*dt]])
#Rk = np.array([[1.0]])
# create EKF object
EKF = ekf.ekf(2,0,eqom_ekf,eqom_jacobian_ekf,eqom_gk_ekf,Qk)
nSteps = int(tf/dt)+1
ts = 0.0
# initialize EKF
EKF.init_P(mux0,P0,ts)
# initialize performance object
simOut = trials_processing.simOutput()
xf = np.zeros((nSteps,2))
Pf = np.zeros((nSteps,2,2))
tk = np.arange(0.0,tf,dt)
xf[0,:] = EKF.xhat.copy()
Pf[0,:,:] = EKF.Pk.copy()
t1 = time.time()
for k in range(1,nSteps):
# get the new measurement
ym = np.array([YK[k]])
ts = ts + dt
# sync the EKF, with continuous-time integration
EKF.propagateOde(dt)
#EKF.propagateRK4(dt)
EKF.update(ts,ym,measurement_ekf,measurement_gradient,Rk)
# check that the eigenvalukes are reasonably bounded
w = np.linalg.eigvalsh(EKF.Pk.copy())
for jj in range(len(w)):
if math.fabs(w[jj]) > 1.0e6:
simOut.fail_singular_covariance(k)
print("Covariance eigenvalue too large, t = %f" % (ts))
return(xf,Pf,simOut)
# copy
xf[k,:] = EKF.xhat.copy()
Pf[k,:,:] = EKF.Pk.copy()
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
simOut.complete(nSteps)
return(xf,Pf,simOut)
def main(argin='./'):
# output file
FOUT = open('python_ekf_test.csv','w')
FOUT.write('t,x1,x2,ymeas,x1hat,x2hat,P11,P22\n');
# initialize EKF
#Qkin = np.array([[0.2]])
Qkin = np.array([[20.0]])
EKF = ekf.ekf(2,1,stateDerivativeEKF,stateGradient,stateProcessInfluence,Qk = Qkin)
Rkin = np.array([ [0.0001] ])
dt = 0.01
tfin = 10.0
nSteps = int(tfin/dt)
tsim = 0.0
xk = np.array([1.0,0.0])
yk = measFunction(xk,tsim)
EKF.init(yk,initFunction,tsim)
print(nSteps)
for k in range(nSteps):
# propagte
EKF.propagate(dt)
# simulate
y = sp.odeint(stateDerivative,xk,np.array([tsim,tsim+dt]),args=([],) )
xk = y[-1,:].copy()
# update time
tsim = tsim + dt
# measurement
ymeas = simMeasurementFunction(xk,tsim)
# update EKF
EKF.update(tsim,ymeas,measFunction,measGradient,Rkin)
# log to file
FOUT.write('%f,%f,%f,%f,%f,%f,%f,%f\n' % (tsim,xk[0],xk[1],ymeas[0],EKF.xhat[0],EKF.xhat[1],EKF.Pk[0,0],EKF.Pk[1,1]) )
FOUT.close()
return
def __init__(self,logDir=None):
## Lasst update time
self.tLast = 0.0
## gpsState object for holding the state of EMILY
self.gpsState = gps_state()
## local copy of the EKF state: [x(m),y(m),V(m/s),heading(radians)]
self.filterState = np.zeros(4)
## time of last message
#
# timeLastMsg[0] = the last message of any type
# timeLastMsg[1] = the last heartbeat
# timeLastMsg[2] = the last GPS message
self.timeLastMsg = [0.0,0.0,0.0]
## Current rudder setting
self.rudderCmd=0.0
## Current throttle
self.throttleCmd=0.0
## HACK Throttle moving average used to see when we're fast enough to filter
self.throttleAvg = 0.0
# process noise matrix for the zero-jerk filter
Qkin = np.diag([math.pow(filter_dynamics.sigma_jerk,2.0),math.pow(filter_dynamics.sigma_jerk,2.0)])
## EKF object
self.EKF = ekf.ekf(6,0,filter_dynamics.propagate,filter_dynamics.propGradient,filter_dynamics.processMatrix,Qk=Qkin)
## control mode: 0 == teleoperation, 1 == pfields
self.control_mode = 0
## log object
if logDir is not None:
self.log = open(logDir+"bridgeStateLog.csv",'w+')
# write the headers
self.log.write('time(sec),raw x,raw y,raw V,raw hdg,X(m),Y(m),VX(m/s),VY(m/s),AX(m/s^2),AY(m/s^2),Xm(m),Ym(m),Vm(m),Hdgm(rad)')
for k in range(6):
for j in range(6):
self.log.write(',P_%d%d' % (k+1,j+1))
self.log.write(',EKF time(sec),systime(sec)')
self.log.write('\n')
else:
self.log = None
return
def ekf_test(dt,tf,mux0,P0,YK,Qk,Rk):
#Qk = np.array([[1.0*dt]])
#Rk = np.array([[1.0]])
# create EKF object
EKF = ekf.ekf(2,0,eqom_ekf,eqom_jacobian_ekf,eqom_gk_ekf,Qk)
nSteps = int(tf/dt)+1
ts = 0.0
# initialize EKF
EKF.init_P(mux0,P0,ts)
xf = np.zeros((nSteps,2))
Pf = np.zeros((nSteps,4))
tk = np.arange(0.0,tf,dt)
xf[0,:] = EKF.xhat.copy()
Pf[0,:] = EKF.Pk.reshape((4,))
t1 = time.time()
for k in range(1,nSteps):
# get the new measurement
ym = np.array([YK[k]])
ts = ts + dt
# sync the EKF, with continuous-time integration
EKF.propagateOde(dt)
#EKF.propagateRK4(dt)
EKF.update(ts,ym,measurement_ekf,measurement_gradient,Rk)
# copy
xf[k,:] = EKF.xhat.copy()
Pf[k,:] = EKF.Pk.reshape((4,))
t2 = time.time()
print("Elapsed time: %f sec" % (t2-t1))
return(xf,Pf)
def state_estimation():
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('imu/data', Imu, imu_callback)
rospy.Subscriber('encoder', Encoder, enc_callback)
rospy.Subscriber('ecu', ECU, ecu_callback)
state_pub = rospy.Publisher('state_estimate', Vector3, queue_size=10)
# get vehicle dimension parameters
L_a = rospy.get_param("L_a") # distance from CoG to front axel
L_b = rospy.get_param("L_b") # distance from CoG to rear axel
m = rospy.get_param("m") # mass of vehicle
I_z = rospy.get_param("I_z") # moment of inertia about z-axis
vhMdl = (L_a, L_b, m, I_z)
# get encoder parameters
dt_vx = rospy.get_param(
"state_estimation/dt_v_enc") # time interval to compute v_x
# get tire model
B = rospy.get_param("tire_model/B")
C = rospy.get_param("tire_model/C")
mu = rospy.get_param("tire_model/mu")
TrMdl = ([B, C, mu], [B, C, mu])
# get external force model
a0 = rospy.get_param("air_drag_coeff")
Ff = rospy.get_param("friction")
# get EKF observer properties
q_std = rospy.get_param("state_estimation/q_std") # std of process noise
r_std = rospy.get_param(
"state_estimation/r_std") # std of measurementnoise
v_x_min = rospy.get_param(
"state_estimation/v_x_min") # minimum velociy before using EKF
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
t0 = time.time()
# estimation variables for Luemberger observer
z_EKF = array([1.0, 0.0, 0.0])
# estimation variables for EKF
P = eye(3) # initial dynamics coveriance matrix
Q = (q_std**2) * eye(3) # process noise coveriance matrix
R = (r_std**2) * eye(2) # measurement noise coveriance matrix
while not rospy.is_shutdown():
# publish state estimate
(v_x, v_y, r) = z_EKF # note, r = EKF estimate yaw rate
# publish information
state_pub.publish(Vector3(v_x, v_y, r))
# apply EKF
if v_x_enc > v_x_min:
# get measurement
y = array([v_x_enc, w_z])
# define input
u = array([d_f, FxR])
# build extra arguments for non-linear function
F_ext = array([a0, Ff])
args = (u, vhMdl, TrMdl, F_ext, dt)
# apply EKF and get each state estimate
(z_EKF, P) = ekf(f_3s, z_EKF, P, h_3s, y, Q, R, args)
else:
z_EKF[0] = float(v_x_enc)
z_EKF[2] = float(w_z)
# wait
rate.sleep()
def state_estimation():
global Vx_meas, Yaw_meas, X_meas, Y_meas, delta_meas
global Yaw_initialize, GPS_initialize, GPS_read
global start_X, start_Y, pub_flag
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('/xsens/imu/data', Imu, IMUCallback)
rospy.Subscriber('vehicle/steering_report', SteeringReport,
SteeringReportCallback)
rospy.Subscriber('current_pose_2D', Pose2D, CurrentPose2DCallback)
rospy.Subscriber('relative_quaternion', Quaternion,
RelativeOrientationCallback)
state_pub = rospy.Publisher('state_estimate', state_Dynamic, queue_size=10)
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
# estimation variables for Luemberger observer
z_EKF = array([0., 0., 0., 0., 0., 0.])
z_EKF_prev = array([0., 0., 0., 0., 0., 0.])
states_est = array([0., 0., 0., 0., 0., 0.])
state_initialize = 0
state_est_obj = state_Dynamic()
# estimation variables for EKF
var_gps = 1.0e-05
var_v = 1.0e-04
var_psi = 1.0e-04
var_ax = 1.0e-03
var_ay = 1.0e-03
var_delta = 1.0e-02
var_noise = 1.0e-01
P = eye(6) # initial dynamics coveriance matrix
Q = diag(
array([var_ax, var_delta, var_noise, var_noise, var_noise,
var_noise])) # process noise coveriance matrix
R2 = diag(array([var_v, var_gps, var_gps,
var_psi])) # measurement noise coveriance matrix
R1 = diag(array([var_v, var_psi, var_ay]))
while (rospy.is_shutdown() != 1):
#print((Yaw_initialize, GPS_initialize))
if Yaw_initialize == 0 or GPS_initialize == 0:
rate.sleep()
continue
elif state_initialize == 0:
z_EKF = array([Vx_meas, 0, X_meas, Y_meas, Yaw_meas, Yawrate_meas])
state_est = z_EKF
state_initialize = 1
else:
factor = 1.0
if abs(Vx_meas) < 0.01:
factor = 0.0
u_ekf = array([ax_meas * factor, delta_meas * factor])
w_ekf = array([0., 0., 0., 0., 0., 0.])
args = (u_ekf, vhMdl, trMdl, dt)
z_EKF_prev = z_EKF
if GPS_read == 0:
y_ekf = array([
Vx_meas * factor,
Yaw_meas * factor + z_EKF[4] * (1. - factor),
ay_meas * factor
])
v_ekf = array([0., 0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withoutGPS, y_ekf, Q, R1, args)
else:
y_ekf = array([Vx_meas, X_meas, Y_meas, Yaw_meas])
v_ekf = array([0., 0., 0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withGPS, y_ekf, Q, R2, args)
GPS_read = 0
state_est = z_EKF
state_est_obj.vx = state_est[0]
state_est_obj.vy = state_est[1]
state_est_obj.X = state_est[2]
state_est_obj.Y = state_est[3]
state_est_obj.psi = state_est[4]
state_est_obj.wz = state_est[5]
if Vx_meas == 0:
state_est_obj.vx = 0
state_est_obj.vy = 0
state_pub.publish(state_est_obj)
rate.sleep()
control = control[ci:,:] # GPS to X-Y position relative to home gpsXY = np.zeros((len(gps),2)) gpsXY[:,0] = (gps[:,3]-gpsHome[1])*111318.845 # X gpsXY[:,1] = (gps[:,2]-gpsHome[0])*111318.845 # Y # 1-sigma in the process noise for heading gain sigma_psi = 0.5 sigma_t = 0.5 sigma_gps = 0.5*3.0/3.0# meters, 1-sigma sigma_gps_v = 0.5/3.0#m/s sigma_gps_h = 0.5/3.0#rads Qkin = np.diag([sigma_psi*sigma_psi,sigma_t*sigma_t]) Rkin = np.diag([sigma_gps*sigma_gps,sigma_gps*sigma_gps,sigma_gps_v*sigma_gps_v,sigma_gps_h*sigma_gps_h]) EKF = ekf.ekf(6,2,propagate,propagateGradient,processNoiseMatrix,Qk=Qkin) # initialize filter xk0 = np.array([gpsXY[0,0],gpsXY[0,1],gps[0,4],gps[0,5],2.0,2.0]) Pk0 = np.diag([sigma_gps*sigma_gps,sigma_gps*sigma_gps,0.5,0.5,sigma_psi*sigma_psi,sigma_t*sigma_t]) EKF.init_P(xk0,Pk0,gps[0,0]) # previous value of GPS time, X, Y, speed, heading (rad) - used for outlier rejection stateLast = np.array([gps[0,0],gpsXY[0,0],gpsXY[0,1],gps[0,4],gps[0,5]]) # count bad GPS rejectCount = 0 # log the state LEN = 600 xLog = np.zeros((LEN,6)) xLog[0,:] = EKF.xhat.copy() Plog = np.zeros((LEN,6)) Plog[0,:] = np.diag(EKF.Pk.copy())
gps[:,0] = gps[:,0]-t0 # Time to start at tstart = 150.0 # truncate the input vectors gi = np.nonzero(gps[:,0]>=tstart)[0][0] ci = np.nonzero(control[:,0]>=tstart)[0][0] gps = gps[gi:,:] control = control[ci:,:] # GPS to X-Y position relative to home gpsXY = np.zeros((len(gps),2)) gpsXY[:,0] = (gps[:,3]-gpsHome[1])*111318.845 # X gpsXY[:,1] = (gps[:,2]-gpsHome[0])*111318.845 # Y Qkin = np.diag([math.pow(sigma_jerk,2.0),math.pow(sigma_jerk,2.0)]) Rkin = np.diag([math.pow(sigma_gps,2.0),math.pow(sigma_gps,2.0)]) EKF = ekf.ekf(6,0,propagate,propGradient,processMatrix,Qk=Qkin) # initialize filter xk0 = np.array([gpsXY[0,0],gpsXY[0,1],0.0,0.0,0.0,0.0]) Pk0 = np.diag([math.pow(sigma_gps,2.0),math.pow(sigma_gps,2.0),1.0,1.0,1.0,1.0]) EKF.init_P(xk0,Pk0,gps[0,0]) # previous value of GPS time, X, Y, speed, heading (rad) - used for outlier rejection stateLast = np.array([gps[0,0],gpsXY[0,0],gpsXY[0,1],gps[0,4],gps[0,5]]) # count bad GPS rejectCount = 0 # log the state LEN = 3000 xLog = np.zeros((LEN,6)) xLog[0,:] = EKF.xhat.copy() Plog = np.zeros((LEN,6)) Plog[0,:] = np.diag(EKF.Pk.copy())
def main():
# load data
filename = '20160328172944.csv'
dat = gpsParser.parseFile(filename)
# shorten the data vector
tst = 186
a0 = np.nonzero((dat[:,0] - dat[0,0]) > tst)
dat = dat[a0[0][0]:,:]
# time vector
tsim = dat[:,0]-dat[0,0]
# X-Y data
Y = dat[:,6:8]
# set the first point to the origin
Y[:,0] = Y[:,0] - Y[0,0]
Y[:,1] = Y[:,1] - Y[0,1]
# measurement noise estimate
Rk = np.diag(math.pow(5.0/3.0,2.0)*np.ones(2,))# metres
# process noise estimate
Qk = np.diag([10.0,10.0])
# measurement interval
dt = 1.0
EKF = ekf.ekf(4,0,stateDerivative,stateGradient,processInfluence,Qk=Qk)
# initialize estimate state
#x0 = np.array([Y[0,0],Y[0,1],1.0,0.0])
x0 = np.array([Y[0,0],Y[0,1],dat[0,5],dat[0,4]*math.pi/180.0])
# initial covariance
P0 = np.diag([Rk[0,0],Rk[1,1],1.0,1.0])
# initialize the EKF state
EKF.init_P(x0,P0,0.0)
# number of steps to propagate
nSteps = 100
# log values
xAll = np.zeros((2*nSteps-1,4))
tAll = np.zeros(2*nSteps-1,)
# log posterior values
xk = np.zeros((nSteps,4))
Pk = np.zeros((nSteps,4))
allCounter = 0
xAll[allCounter,:] = x0.copy()
tAll[allCounter] = tsim[0]
for k in range(0,nSteps):
if k > 0:
dt = tsim[k]-tsim[k-1]
# propagate
EKF.propagateOde(dt)
# log
allCounter = allCounter+1
xAll[allCounter,:] = EKF.xhat.copy()
tAll[allCounter] = tsim[k]
# update
EKF.update(tsim[k],Y[k,:],measurementFunction,measurementGradient,Rk)
# log all
allCounter = allCounter+1
xAll[allCounter,:] = EKF.xhat.copy()
tAll[allCounter] = tsim[k]
print(allCounter)
# log
xk[k,:] = EKF.xhat.copy()
Pk[k,:] = np.diag(EKF.Pk.copy())
# compute propagataion residuals
# plot results
fig1 = plt.figure()
lbls = ['x(m)','y(m)','v(m/s)','hdg(rad)']
tplot = tsim[:nSteps]
ax = []
for k in range(4):
ax.append(fig1.add_subplot(2,2,k+1,ylabel=lbls[k]))
#ax[k].plot(tplot,xk[:,k])
ax[k].plot(tAll,xAll[:,k])
if k < 2:
ax[k].plot(tsim,Y[:,k],'r--')
elif k == 2:
ax[k].plot(tsim,dat[:,5],'r--')
elif k == 3:
ax[k].plot(tsim,dat[:,4]*math.pi/180.0,'r--')
fig1.show()
fig2 = plt.figure()
lbls = ['x(m)','y(m)','v(m/s)','hdg(rad)']
tplot = tsim[:nSteps]
# residuals
e = np.zeros((nSteps,4))
e[:,0] = xk[:,0] - Y[:nSteps,0]
e[:,1] = xk[:,1] - Y[:nSteps,1]
e[:,2] = xk[:,2] - dat[:nSteps,5]
e[:,3] = xk[:,3] - dat[:nSteps,4]*math.pi/180.0
ax = []
for k in range(4):
ax.append(fig2.add_subplot(2,2,k+1,ylabel=lbls[k]))
ax[k].plot(tplot,e[:,k])
ax[k].plot(tplot,3.0*np.sqrt(Pk[:,k]),'r--')
ax[k].plot(tplot,-3.0*np.sqrt(Pk[:,k]),'r--')
fig2.show()
raw_input('Return to exit')
plt.close(fig1)
# path_now.pose.position.x = xpos
# path_now.pose.position.y = ypos
# path_now.pose.position.z = 0
# travelPath.poses.append(path_now)
# path.publish(travelPath)
if __name__ == '__main__':
#set up the node
rospy.init_node('moveROVekf', anonymous=True)
# tn = rospy.Time.now()
broadcaster = tf.TransformBroadcaster()
listener = tf.TransformListener()
ekffilter = ekf.ekf()
dvl_callback.initialized = False
mti_callback.initialized = False
sonar_callback.initialized = False
xpos = 0
ypos = 0
zpos = 0
yawpos = 0
xbuff = 0
ybuff = 0
zbuff = 0
yawbuff = 0
travelPath = Path()
rospy.init_node("ekf")
sub_pos = rospy.Subscriber("/slam_out_pose", PoseStamped, newPos)
sub_wheelEnc = rospy.Subscriber("/wheel_encoder", WheelEncoder,
wheelEnc_callback)
pub_ekf = rospy.Publisher("/ekf_estimate", PoseStamped, queue_size=1)
h = 1 / 25.0
rate = rospy.Rate(1 / h)
pose_point = PoseStamped()
Q = np.diag([0.001, 0.001, 0.01, 0.01, 0.01])
#Qsq = h*sl.cholesky(Q)
R = np.diag([0.01, 0.01, 1.0, 1.0])
#Rsq = sl.cholesky(R)
# EKF STUFF
my_ekf = ekf.ekf(Q, R)
## Run the estimation on the generated data
#m = np.zeros((N,5,1))
#P = np.zeros((N,5,5))
m0 = np.zeros((5, 1)) + 1.0
P0 = 0.1 * np.eye(5)
yk0 = np.zeros((4, 1))
yk = np.zeros((4, 1))
mk, Pk = my_ekf.filter_update(yk, m0, P0)
# END EKF STUFF
while not rospy.is_shutdown():
print("ekf'in")
def task(prev_time, error_cov, count, x):
global distance_from_groundtruth
x2=window.winfo_rootx()+main_window.canvas.winfo_x()
y2=window.winfo_rooty()+main_window.canvas.winfo_y()
x1=x2+main_window.canvas.winfo_width()
y1=y2+main_window.canvas.winfo_height()
N = 1000
X = np.linspace(x1, x2, N)
Y = np.linspace(y1, y2, N)
X, Y = np.meshgrid(X, Y)
# Pack X and Y into a single 3-dimensional array
pos = np.empty(X.shape + (4,))
pos[:, :, 0] = X
pos[:, :, 1] = Y
# It converts the RGB color space of image to HSV color space
img=ImageGrab.grab((x2,y2,x1,y1))
frame = np.array(img)
hsv = cv2.cvtColor(frame, cv2.COLOR_RGB2HSV)
# preparing the mask to overlay
mask_blue = cv2.inRange(hsv, lower_blue, upper_blue)
mask_green = cv2.inRange(hsv, lower_green, upper_green)
mask_yellow = cv2.inRange(hsv, lower_yellow, upper_yellow)
mask_red = cv2.inRange(hsv, lower_red, upper_red)
masks=[mask_blue, mask_green, mask_yellow, mask_red]
for c in range(len(masks)):
M=cv2.moments(masks[c])
contours,hierarchy = cv2.findContours(masks[c].copy(), 1, 2)
if len(contours)!=0:
covered[c]=4
area=cv2.contourArea(contours[0])
if M["m00"]!=0 and (area>1500 or area==0 or (area>100 and c==3)):
cX[c] = int(M["m10"] / M["m00"])
cY[c] = int(M["m01"] / M["m00"])
meas[c].append((cX[c],cY[c]))
filterstep[c]=time.time()-prev_time[c]
prev_time[c]=time.time()
prev_cov=error_cov[c].copy()
x[c], error_cov[c] = ekf([cX[c],cY[c]], x[c], filterstep[c], error_cov[c], count)
pred[c].append((int(x[c][0]),int(x[c][1])))
error[c].append(np.trace(error_cov[c]))
error_kl[c].append(distance_kullback(prev_cov, error_cov[c]))
data="{}: actual- {} \t predicted- {} \t error (trace)- {} \t error (kl)- {}".format(c, (cX[c],cY[c]), (int(x[c][0]),int(x[c][1])), error[c][-1], error_kl[c][-1])
outF.write(data)
outF.write("\n")
distance_from_groundtruth+=[math.hypot(int(x[c][0]-cX[c]), int(x[c][1]-cY[c]))]
paint(c)
# F = multivariate_normal(np.array(x[c]), error_cov[c])
# Z = F.pdf(pos)
# plt.contourf(X, Y, Z, cmap=cm.viridis)
# plt.show(block=False)
else:
if covered[c]==4:
distances=dict()
for i in range(len(masks)):
if i!=c:
distances[i]=bhattacharyya(x[c], error_cov[c], x[i], error_cov[i])
covered[c]=min(distances, key=distances.get)
prev_cov=error_cov[c].copy()
error_cov[c]*=1.05
pred[c].append((int(x[covered[c]][0]),int(x[covered[c]][1])))
error[c].append(np.trace(error_cov[c]))
error_kl[c].append(distance_kullback(prev_cov, error_cov[c]))
data="{}: actual- {} \t predicted- {} \t error (trace)- {} \t error (kl)- {}".format(c, (cX[c],cY[c]), (int(x[c][0]),int(x[c][1])), error[c][-1], error_kl[c][-1])
outF.write(data)
outF.write("\n")
distance_from_groundtruth+=[math.hypot(int(x[c][0]-cX[c]), int(x[c][1]-cY[c]))]
paint(c)
cv2.imshow("kalman",frame_kalman)
count+=1
window.after(10, task, prev_time, error_cov, count, x) # reschedule event
def state_estimation():
global Vx_meas, Yaw_meas, delta_meas
global start_X, start_Y, pub_flag
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('/xsens/imu_data', Imu, IMUCallback)
rospy.Subscriber('vehicle/steering_report', SteeringReport,
SteeringReportCallback)
state_pub = rospy.Publisher('state_estimate', state_Dynamic, queue_size=10)
# set node rate
loop_rate = 100
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
# estimation variables for Luemberger observer
z_EKF = array([0., 0., 0., 0., 0., 0.])
z_EKF_prev = array([0., 0., 0., 0., 0., 0.])
states_est = array([0., 0., 0., 0., 0., 0.])
state_initialize = 0
state_est_obj = state_Dynamic()
# estimation variables for EKF
var_v = 1.0e-04
var_psi = 1.0e-06
var_ax = 1.0e-04
var_delta = 1.0e-04
var_noise = 1.0e-04
P = eye(6) # initial dynamics coveriance matrix
Q = diag(
array([var_ax, var_delta, var_noise, var_noise, var_noise,
var_noise])) # process noise coveriance matrix
R1 = diag(array([var_v, var_psi]))
while (rospy.is_shutdown() != 1):
if state_initialize == 0:
z_EKF = array([Vx_meas, 0, 0, 0, Yaw_meas, Yawrate_meas])
state_est = z_EKF
state_initialize = 1
else:
u_ekf = array([ax_meas, delta_meas])
w_ekf = array([0., 0., 0., 0., 0., 0.])
args = (u_ekf, vhMdl, trMdl, dt)
z_EKF_prev = z_EKF
y_ekf = array([Vx_meas, Yaw_meas])
v_ekf = array([0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withoutGPS, y_ekf, Q, R1, args)
state_est = z_EKF
state_est_obj.vx = state_est[0]
state_est_obj.vy = state_est[1]
state_est_obj.X = state_est[2]
state_est_obj.Y = state_est[3]
state_est_obj.psi = state_est[4]
state_est_obj.wz = state_est[5]
state_pub.publish(state_est_obj)
rate.sleep()
# path_now.pose.position.y = ypos
# path_now.pose.position.z = 0
# travelPath.poses.append(path_now)
# path.publish(travelPath)
if __name__ == '__main__':
#set up the node
rospy.init_node('moveROVekf', anonymous=True)
# tn = rospy.Time.now()
broadcaster = tf.TransformBroadcaster()
listener = tf.TransformListener()
ekffilter = ekf.ekf()
dvl_callback.initialized = False
mti_callback.initialized = False
sonar_callback.initialized = False
xpos = 0
ypos = 0
zpos = 0
yawpos = 0
xbuff = 0
ybuff = 0
zbuff = 0
yawbuff = 0
travelPath = Path()
def state_estimation():
global Vx_meas, Yaw_meas, delta_meas, ax_meas, ay_meas
global start_X, start_Y, pub_flag, steering_received, imu_received
# initialize node
rospy.init_node('state_estimation', anonymous=True)
# topic subscriptions / publications
rospy.Subscriber('/xsens/imu/data', Imu, IMUCallback)
rospy.Subscriber('vehicle/steering_report', SteeringReport,
SteeringReportCallback)
state_pub = rospy.Publisher('state_estimate', state_Dynamic, queue_size=1)
# set node rate
loop_rate = 50
dt = 1.0 / loop_rate
rate = rospy.Rate(loop_rate)
# estimation variables for Luemberger observer
z_EKF = array([0., 0., 0., 0., 0., 0.])
z_EKF_prev = array([0., 0., 0., 0., 0., 0.])
states_est = array([0., 0., 0., 0., 0., 0.])
state_initialize = 0
state_est_obj = state_Dynamic()
# estimation variables for EKF
var_v = 1.0e-04
var_psi = 1.0e-03
var_ax = 1.0e-02
var_ay = 1.0e-02
var_delta = 1.0e-02
var_noise = 1.0e-01
P = eye(6) # initial dynamics coveriance matrix
Q = diag(
array([var_ax, var_delta, var_noise, var_noise, var_noise,
var_noise])) # process noise coveriance matrix
R1 = diag(array([var_v, var_psi, var_ay]))
while (rospy.is_shutdown() != 1):
if steering_received and imu_received:
steering_received = False
imu_received = False
if state_initialize == 0:
#z_EKF = array([Vx_meas, 0, 0, 0, Yaw_meas, Yawrate_meas])
z_EKF = array([0, 0, 0, 0, Yaw_meas, 0])
state_est = z_EKF
state_initialize = 1
else:
factor = 1.0
if abs(Vx_meas) < 0.01:
factor = 0.0
while (Yaw_meas - state_est[4] > pi):
Yaw_meas -= 2 * pi
while (Yaw_meas - state_est[4] < -pi):
Yaw_meas += 2 * pi
u_ekf = array([ax_meas * factor, delta_meas * factor])
w_ekf = array([0., 0., 0., 0., 0., 0.])
args = (u_ekf, vhMdl, trMdl, dt)
z_EKF_prev = z_EKF
y_ekf = array([
Vx_meas * factor,
Yaw_meas * factor + z_EKF[4] * (1. - factor),
ay_meas * factor
])
v_ekf = array([0., 0., 0.])
(z_EKF, P) = ekf(f_BicycleModel, z_EKF, w_ekf, v_ekf, P,
h_BicycleModel_withoutGPS, y_ekf, Q, R1, args)
z_EKF[4] = z_EKF[4] % (2 * pi)
if z_EKF[4] > pi:
z_EKF[4] -= 2 * pi
state_est = z_EKF
state_est_obj.vx = state_est[0]
state_est_obj.vy = state_est[1]
state_est_obj.X = state_est[2]
state_est_obj.Y = state_est[3]
state_est_obj.psi = state_est[4]
state_est_obj.wz = state_est[5]
state_pub.publish(state_est_obj)
rate.sleep()
for c in range(len(masks)):
M = cv2.moments(masks[c])
contours, hierarchy = cv2.findContours(masks[c].copy(), 1, 2)
if len(contours) != 0:
covered[c] = 4
area = cv2.contourArea(contours[0])
if M["m00"] != 0 and (area > 1500 or area == 0 or
(area > 100 and c == 3)):
cX[c] = int(M["m10"] / M["m00"])
cY[c] = int(M["m01"] / M["m00"])
meas[c].append((cX[c], cY[c]))
filterstep[c] = time.time() - prev_time[c]
prev_time[c] = time.time()
prev_cov = error_cov[c].copy()
x[c], error_cov[c] = ekf([cX[c], cY[c]], x[c], filterstep[c],
error_cov[c], count)
pred[c].append((int(x[c][0]), int(x[c][1])))
error[c].append(np.trace(error_cov[c]))
error_kl[c].append(distance_kullback(prev_cov, error_cov[c]))
else:
if covered[c] == 4:
distances = dict()
for i in range(len(masks)):
if i != c and np.any(error_cov[i]):
distances[i] = bhattacharyya(
x[c], error_cov[c], x[i], error_cov[i])
covered[c] = min(distances, key=distances.get)
meas[c].append(meas[covered[c]][-1])
cX[c] = meas[covered[c]][-1][0]
cY[c] = meas[covered[c]][-1][1]
def main(argin='./'):
# output file
FOUT = open('python_benchmark.csv','w')
FOUT.write('t,x1,x2,ymeas1,ymeas2,x1hat,x2hat,P11,P22\n');
# initialize EKF
Qkin = np.array([[0.2]])
#Qkin = np.array([[20.0]])
EKF = ekf.ekf(2,1,stateDerivativeEKF,stateGradient,stateProcessInfluence,Qk = Qkin)
Rkin = np.array([ [sigma_y1*sigma_y1] ])
dt = 0.01
tfin = 10.0
nSteps = int(tfin/dt)
tsim = 0.0
xk = np.array([1.0,0.0])
yk = measFunction(xk,tsim)
EKF.init(yk,initFunction,tsim)
print(nSteps)
xkl = np.zeros((nSteps,2))
xtl = np.zeros((nSteps,2))
Pkl = np.zeros((nSteps,2))
tl = np.zeros((nSteps,))
for k in range(nSteps):
# propagte
EKF.propagateRK4(dt)
# simulate
y = sp.odeint(stateDerivative,xk,np.array([tsim,tsim+dt]),args=([],) )
xk = y[-1,:].copy()
# update time
tsim = tsim + dt
# measurement
ymeas = simMeasurementFunction(xk,tsim)
# update EKF
EKF.update(tsim,ymeas,measFunction,measGradient,Rkin)
# log to file
FOUT.write('%f,%f,%f,%f,%f,%f,%f,%f\n' % (tsim,xk[0],xk[1],ymeas[0],EKF.xhat[0],EKF.xhat[1],EKF.Pk[0,0],EKF.Pk[1,1]) )
# log to data
xkl[k,0] = EKF.xhat[0]
xkl[k,1] = EKF.xhat[1]
xtl[k,0] = xk[0]
xtl[k,1] = xk[1]
Pkl[k,0] = EKF.Pk[0,0]
Pkl[k,1] = EKF.Pk[1,1]
tl[k] = tsim
print("Completed sim")
fig = plt.figure()
ax = []
for k in range(2):
nam = 'e'+str(k+1)
ax.append(fig.add_subplot(2,1,k+1,ylabel=nam))
ax[k].plot(tl,xkl[:,k]-xtl[:,k])
ax[k].plot(tl,3*np.sqrt(Pkl[:,k]),'r--')
ax[k].plot(tl,-3*np.sqrt(Pkl[:,k]),'r--')
ax[k].grid()
fig.show()
raw_input("Return to continue")
FOUT.close()
return
