def avgqtn(set_q, weight, sigma=0.0001):
'''import set quaternion and set weight, error sigma, starting point
#q1 = euler.euler2quat(0, 0, 170/180*math.pi)
#q2 = euler.euler2quat(0, 0, -101/180*math.pi)
#q3 = euler.euler2quat(0, 0, 270/180*math.pi)
#set_q = [q1, q2, q3]
#weight = [1/3, 1/3, 1/3]
#a = avgqtn(set_q, weight)
#k = euler.quat2euler(a)
'''
qt_bar = set_q[0]
#weight = [0, 1/6, 1/6, 1/6, 1/6, 1/6, 1/6]
n = len(weight) #size of weight
while True:
ev = np.zeros((n, 3)) # n is the size of the set of Q n*3
for q, w, i in zip(set_q, weight, range(len(weight))):
qi_e = mul(inv(qt_bar), q)
qie = np.array(qi_e)/norm(qi_e)
e = np.array(2)* log(qie.tolist()) # 4 dimension
ev_i = e[1:4] # 3 dimension
if norm(ev_i) != 0:
ev_fi = (-math.pi + math.fmod(norm(ev_i) + math.pi, math.pi * 2)) / \
norm(ev_i) * np.array(ev_i) * w
ev[i, :] = ev_fi
E = np.sum(ev, axis=0) #final ev after summation 1*n
qt1_bar = mul(qt_bar, exp([0, E[0]/2, E[1]/2, E[2]/2]))
qt_bar = qt1_bar #list
if norm(E) < sigma:
break
return qt_bar
def prediction(qt, E, step): '''where Rq_imu_step is the quaternion rotation for each step based on t predict t+1 this function is based on sigma point, noise added ''' #motion model qt1 = [] e = [] #error vector for qt+1 7 for Ei in E: #predict each sigma point from time t to t+1 qt1_i = qtn.mul(qt, qtn.mul(qtn.exp ([0, 0.5 * Ei[0], 0.5 * Ei[1], 0.5 * Ei[2]]), qtn.exp(step))) qt1.append(qt1_i) #qt1 means at qt+1 state there is 7 sigma points #in qt+1 the first element is the sigma point transformed from estimated qt mean #calculate the mean and covariance weight = [0, 1 / 6, 1 / 6, 1 / 6, 1 / 6, 1 / 6, 1 / 6] qt_1_bar = avgqtn(qt1, weight) #mean value of the predicted sigma point, a list value cov = np.zeros((3, 3)) #covariance is 3*3 matrix since quaternion is shrinked from 4 to 3 for qt1_i, i in zip(qt1, range(len(qt1))): #q = qtn.mul(qt1_i, qtn.inv(qt_1_bar)) q = qtn.mul(qtn.inv(qt_1_bar), qt1_i) ei = qtn.d4tod3(q) # ei is a 3 dimension list eiv = np.array(ei).reshape((3,1)) e.append(eiv) if i == 0: cov = 2 * np.dot(eiv, eiv.T) else: cov = cov + 1/6 * np.dot(eiv, eiv.T) #covariance value of the predicted sigma point return qt_1_bar, cov, qt1, e # where the qt1bar is the mean of the state qt+1 predicted
def update(kt1, imu_acc_t1, zt1_bar, cov_vv, Pqt1_bar, cov_predicted): '''kt1 is a 3*3 matrix''' inov_t1 = np.array (imu_acc_t1 - zt1_bar) k_inovt1 = np.dot(kt1, inov_t1) qt1_updated = qtn.mul(Pqt1_bar, qtn.exp([0, 0.5*k_inovt1[0], 0.5*k_inovt1[1], 0.5*k_inovt1[2]])) k_Pvv = np.dot(kt1 , cov_vv) covt1_updated = cov_predicted - np.dot(k_Pvv, kt1.T) return qt1_updated, covt1_updated
def measurement (qt1): '''with predicted qt+1, and imu_acc at this qt+1 step''' #measurement zt1 = [] #which is my measurement #noise_t = np.array(imu_acc_step) - np.array(zt_bar) # zt - zt_bar for qt1_i in qt1: # seven element qzt1_i = qtn.mul(qtn.mul(qtn.cjgt(qt1_i), [0, 0, 0, 1]), qt1_i) #world to body frame qt-1 *v * qt, if it is body to world qt * v * qt-1 zt1_i = np.array(qzt1_i[1:4]) #+ noise_t #noise is 3d zt1.append(zt1_i.tolist()) zt1_bar = 1/6 * np.sum(zt1[1:7], axis = 0) + 0 * np.array(zt1[0]) covzz = np.zeros((3,3)) for zt1_i, i in zip(zt1, range(len(zt1))): z = np.array(zt1_i) - zt1_bar zt1_iv = np.array(z).reshape((3, 1)) if i == 0: covzz = 2 * np.dot(zt1_iv, zt1_iv.T) else: covzz = covzz + 1/6 * np.dot(zt1_iv, zt1_iv.T) #covariance cov_vv = covzz + 0.0001*np.eye(3) return zt1_bar, cov_vv, zt1
for i in range(num - 1):
accx_imu = -1 * (imu_values[0, i + 1] - biasacc[0]) * scale_factorg
accy_imu = -1 * (imu_values[1, i + 1] - biasacc[1]) * scale_factorg
accz_imu = (imu_values[2, i + 1] - biasacc[2]) * scale_factorg
acc_imu = [accx_imu, accy_imu, accz_imu] #each imu step for acc
Wz_imu = 0.5 * (imu_values[3, i] - biasW[0]) * scale_factor * (
imu_ts[0, i + 1] - imu_ts[0, i])
Wx_imu = 0.5 * (imu_values[4, i] - biasW[1]) * scale_factor * (
imu_ts[0, i + 1] - imu_ts[0, i])
Wy_imu = 0.5 * (imu_values[5, i] - biasW[2]) * scale_factor * (
imu_ts[0, i + 1] - imu_ts[0, i])
W_imu = [Wx_imu, Wy_imu, Wz_imu]
Rq_imu = [0] + W_imu # rotaion in quaternion
qt_1 = qtn.mul(qt, qtn.exp(Rq_imu)) # orientation at state t_1
qt = qt_1
euler_qt_1 = quat2euler(qt_1) # xyz
imu_orienstate.append(
euler_qt_1) # transform the quaternion orientation in to euler states
ypr = mat2euler(
vic_rots[:, :, i]) # xyz transform the rotation matrix into euler
vic_orienstate.append(ypr) # get series of the euler state of vicomn
E = PM.sigma(covt_t, Q)
Pqt_1_bar, Pcov, Pqt1, e = PM.prediction(ut_t, E, Rq_imu)
#qt+1 bar is the predicted mean, cov is the predicted mean, Pqt1 is the predicted 7 sigma points
#measurement
zt1_bar, cov_vv, zt1 = PM.measurement(Pqt1)
#E_qt1 = PM.sigma(Pcov, Q)
#kalem gain
