def sim(self, states, sensor_std, plotType, plotBool, printBool, maxIter=20):
"""
使用模拟的观测值验证算法的准确性
:param states: 【list】模拟的真实状态，可以有多个不同的状态
:param sensor_std: 【float】sensor的噪声标准差[mG]
:param plotType: 【tuple】描绘位置的分量 'xy' or 'yz'
:param plotBool: 【bool】是否绘图
:param printBool: 【bool】是否打印输出
:param maxIter: 【int】最大迭代次数
:return: 【tuple】 位置[x, y, z]和姿态ez的误差百分比
"""
# self.ukf.x = state0.copy() # 初始值
state = states[0] # 真实值
# simData = self.generate_data(state, sensor_std, printBool)
simData = generate_data(self.measureNum, state, sensor_std, printBool)
err_pos, err_em = (0, 0)
for i in range(maxIter):
if plotBool:
print('=========={}=========='.format(i))
plt.ion()
plotP(self, state, i, plotType)
if i == maxIter - 1:
plt.ioff()
plt.show()
self.run(simData, printBool)
posTruth, emTruth = state[:3], q2R(state[3: 7])[:, -1]
pos, em = self.ukf.x[:3], q2R(self.ukf.x[3: 7])[:, -1]
err_pos = np.linalg.norm(pos - posTruth) / np.linalg.norm(posTruth)
err_em = np.linalg.norm(em - emTruth) # 方向矢量本身是归一化的
print('err_pos={:.0%}, err_em={:.0%}'.format(err_pos, err_em))
return (err_pos, err_em)
def sim(states,
state0,
sensor_std,
plotType,
plotBool,
printBool,
maxIter=100):
'''
使用模拟的观测值验证算法的准确性
:param states: 模拟的真实状态
:param state0: 模拟的初始值
:param sensor_std: sensor的噪声标准差[mG]
:param plotType: 【tuple】描绘位置的分量 'xy' or 'yz'
:param plotBool: 【bool】是否绘图
:param printBool: 【bool】是否打印输出
:param maxIter: 【int】最大迭代次数
:return: 【tuple】 位置[x, y, z]和姿态ez的误差百分比
'''
m, n = 9, 7
for i in range(1):
# run
output_data = generate_data(m, states[i], sensor_std, printBool)
LM(state0, output_data, n, maxIter, printBool)
if plotBool:
# plot pos and em
# 最大化窗口
mng = plt.get_current_fig_manager()
mng.window.state('zoomed')
iters = len(poss)
for j in range(iters):
state00 = np.concatenate((poss[j], ems[j]))
plt.ion()
plotPos(state00, states[i], j, plotType)
if j == iters - 1:
plt.ioff()
plt.show()
# plotLM(residual_memory, us)
posTruth, emTruth = states[0][:3], q2R(states[0][3:7])[:, -1]
err_pos = np.linalg.norm(poss[-1] -
posTruth) / np.linalg.norm(posTruth)
err_em = np.linalg.norm(q2R(ems[-1])[:, -1] - emTruth) # 方向矢量本身是归一化的
print('pos={}， emz={}: err_pos={:.0%}, err_em={:.0%}'.format(
np.round(posTruth, 3), np.round(emTruth, 3), err_pos, err_em))
residual_memory.clear()
us.clear()
poss.clear()
ems.clear()
return (err_pos, err_em)
def update(self, output_data):
fitness = []
for it in range(max_iter):
w = ws + (we - ws) * (it / max_iter)
for pi in range(pN):
# 更新每个粒子的位置
self.V[pi] = w * self.V[pi] + c1 * random.uniform(0, 1) * (self.pbest[pi] - self.X[pi]) + c2 * random.uniform(0, 1) * (self.gbest - self.X[pi])
self.X[pi] += self.V[pi]
# 检查是否越界
for j in range(3):
self.X[pi][j] = max(self.posMin, self.X[pi][j])
self.X[pi][j] = min(self.posMax, self.X[pi][j])
# 更新pbest和gbest
tmp = self.fun(self.X[pi], output_data)
if tmp < self.pCost[pi]:
self.pCost[pi] = tmp
self.pbest[pi] = self.X[pi]
if tmp < self.gCost:
self.gCost = tmp
self.gbest = self.X[pi]
fitness.append(self.gCost)
pos = self.gbest[:3]
ez = q2R(self.gbest[3:7])[:,-1]
print('i={}: pos={}, ez={}, cost={}'.format(it + 1, np.round(pos, 3), np.round(ez, 3), self.gCost))
def __init__(self, sensor_std, state0, state):
'''
预测量：x, y, z, q0, q1, q2, q3
:param sensor_std:【float】sensor的噪声标准差[μV]
:param state0: 【np.array】初始值 (7,)
:param state: 【np.array】预测值 (7,)
'''
self.stateNum = 7 # 预测量：x, y, z, q0, q1, q2, q3
self.dt = 0.01 # 时间间隔[s]
self.points = MerweScaledSigmaPoints(n=self.stateNum,
alpha=0.3,
beta=2.,
kappa=3 - self.stateNum)
self.ukf = UKF(dim_x=self.stateNum,
dim_z=self.measureNum,
dt=self.dt,
points=self.points,
fx=self.f,
hx=self.h)
self.ukf.x = state0.copy() # 初始值
self.x0 = state # 计算NEES的真实值
self.ukf.R *= sensor_std
self.ukf.P = np.eye(self.stateNum) * 0.01
for i in range(3, 7):
self.ukf.P[i, i] = 0.01
self.ukf.Q = np.eye(
self.stateNum) * 0.001 * self.dt # 将速度作为过程噪声来源，Qi = [v*dt]
for i in range(3, 7):
self.ukf.Q[i, i] = 0.01 # 四元数的过程误差
self.pos = (round(self.ukf.x[0], 3), round(self.ukf.x[1],
3), round(self.ukf.x[2], 3))
self.m = q2R(self.ukf.x[3:7])[:, -1]
def h(self, state):
dArray0 = state[:3] - self.coilArray
em2 = np.array(q2R(state[3:7]))[:, -1]
E = np.zeros(self.measureNum)
for i, d in enumerate(dArray0):
# E[i * 3] = inducedVolatage(d=d, em1=(1, 0, 0), em2=em2)
# E[i * 3 + 1] = inducedVolatage(d=d, em1=(0, 1, 0), em2=em2)
# E[i * 3 + 2] = inducedVolatage(d=d, em1=(0, 0, 1), em2=em2)
E[i] = inducedVolatage(d=d, em1=(0, 0, 1), em2=em2)
return E
def statePrint(self, ):
self.pos = np.round(self.ukf.x[:3], 3)
self.m = np.round(q2R(self.ukf.x[3:7])[:, -1], 3)
timeCost = (datetime.datetime.now() - self.t0).total_seconds()
Estate = self.h(self.ukf.x) # 计算每个状态对应的感应电压
# 计算NEES值
x = self.x0 - self.ukf.x
nees = np.dot(x.T, np.linalg.inv(self.ukf.P)).dot(x)
print(
'pos={}m, emz={}, Emax={:.2f}, Emin={:.2f}, NEES={:.1f}, timeCost={:.3f}s'
.format(self.pos, self.m, max(abs(Estate)), min(abs(Estate)), nees,
timeCost))
def generateEsim(state, sensor_std, maxNum=50):
'''
根据胶囊的状态给出接收线圈的感应电压模拟值
:param state: 【np.array】 胶囊的真实状态 (7, )
:param maxNum: 【int】最大数据量
:param sensor_std: 【float】传感器噪声，此处指感应电压的采样噪声[μV]
:return: 【np.array】感应电压模拟值 (Tracker.measureNum, maxNum)
'''
E = np.zeros(Tracker.measureNum)
Esim = np.zeros((Tracker.measureNum, maxNum))
em2Sim = q2R(state[3:7])[:, -1]
dArray = state[:3] - Tracker.coilArray
for i, d in enumerate(dArray):
E[i] = inducedVolatage(d=d, em2=em2Sim) # 单向线圈阵列产生的感应电压中间值
for j in range(Tracker.measureNum):
Esim[j, :] = np.random.normal(E[j], sensor_std, maxNum)
return Esim
def h0(state):
'''
胶囊只有内一个磁sensor
:param state: 胶囊的位姿状态
:param EPM: 外部大磁体的朝向
:return: 胶囊内imu+磁传感器的读数
'''
pos, q = state[0:3], state[3:7]
q /= np.linalg.norm(q)
R = q2R(q)
emz = R[:, -1] # 重力矢量在胶囊坐标系下的坐标
# 外部大磁体
EPMNorm = np.linalg.norm(EPM_ORI)
eEPM = EPM_ORI / EPMNorm
r = pos.reshape(3, 1) + EPM_POS
rNorm = np.linalg.norm(r)
er = r / rNorm
# 下线圈
eCOIL = np.array([[0, 0, 1]]).T
rCOIL = pos.reshape(3, 1) + COIL_POS
rCOILNorm = np.linalg.norm(rCOIL)
erCOIL = rCOIL / rCOILNorm
# EPM坐标系下sensor的B值[Gs]
B0 = MOMENT * np.dot(rNorm**(
-3), np.subtract(3 * np.dot(np.inner(er, eEPM), er), eEPM)) / 1000
Bi = COIL_MOMENT * np.dot(
rCOILNorm**(-3),
np.subtract(3 * np.dot(np.inner(erCOIL, eCOIL), erCOIL), eCOIL)) / 1000
# 变换到胶囊坐标系下的sensor读数
B0s = np.dot(R, B0)
B0s[-1] *= -1
Bis = np.dot(R, Bi)
Bis[-1] *= -1
# 加速度计的读数
a_s = emz
return np.concatenate((a_s, B0s.reshape(-1), Bis.reshape(-1)))
def generate_data(self, state, sensor_std, printBool):
"""
生成模拟数据
:param state: 【np.array】模拟的胶囊状态 (m,)
:param sensor_std：【float】磁传感器的噪声标准差 [mG]
:param printBool: 【bool】是否打印输出
:return: 【np.array】模拟的B值 + 加速度计的数值, (num_data, )
"""
Bmid = h(state)[:-2] # 模拟B值数据的中间值
Bsim = np.zeros(SLAVES * 3)
for j in range(SLAVES * 3):
Bsim[j] = np.random.normal(Bmid[j], sensor_std, 1)
R = q2R(state[3: 7])
accSim = R[:, -1]
if printBool:
print('Bmid={}'.format(np.round(Bmid, 0)))
print('Bsim={}'.format(np.round(Bsim, 0)))
print('truth: pos={}m, e_moment={}\n'.format(state[:3], np.round(accSim, 3)))
return np.concatenate((Bsim, accSim))
def run(self, z, printBool):
pos = np.round(self.ukf.x[:3], 3)
em = q2R(self.ukf.x[3: 7])[:, -1]
timeCost = (datetime.datetime.now() - self.t0).total_seconds()
if printBool:
print(r'pos={}m, em={}, w={}, timeCost={:.3f}s'.format(pos, np.round(em, 3),
np.round(self.ukf.x[-3:], 3), timeCost))
self.t0 = datetime.datetime.now()
# 使用IMU的数据更新滤波器
# for i in range(20):
# self.mp.IMUupdate(z[6: 9], z[-3:])
# emq = q2R(self.mp.q)[-1]
# print(r'IMU update: pos={}m, em={}, w={}'.format(pos, np.round(emq, 3), np.round(z[6: 9], 3)))
# self.ukf.x[3: 7] = self.mp.q
# self.ukf.x[7:] = z[6: 9]
# 使用磁传感器的数据更新滤波器
self.ukf.predict()
self.ukf.update(z)
def h(state):
'''
以外部大磁体为参考系，得出胶囊内sensor的读数
:param state: 胶囊的位姿状态
:return: 两个sensor的读数 + 胶囊z轴朝向 (9, )
'''
EPMNorm = np.linalg.norm(EPM_ORI)
eEPM = EPM_ORI / EPMNorm
pos, q = state[0:3], state[3:7]
q /= np.linalg.norm(q)
R = q2R(q)
emz = R[:, -1] # 重力矢量在胶囊坐标系下的坐标
d = np.dot(R.T, SENSORLOC * 0.5) # 胶囊坐标系下的sensor位置矢量转换到EPM坐标系
r1 = pos.reshape(3, 1) + d + EPM_POS
r2 = pos.reshape(3, 1) - d + EPM_POS
r1Norm = np.linalg.norm(r1)
r2Norm = np.linalg.norm(r2)
er1 = r1 / r1Norm
er2 = r2 / r2Norm
# EPM坐标系下每个sensor的B值[Gs]
B1 = MOMENT * np.dot(
r1Norm**(-3), np.subtract(3 * np.dot(np.inner(er1, eEPM), er1),
eEPM)) / 1000
B2 = MOMENT * np.dot(
r2Norm**(-3), np.subtract(3 * np.dot(np.inner(er2, eEPM), er2),
eEPM)) / 1000
# 变换到胶囊坐标系下的sensor读数
B1s = np.dot(R, B1)
B1s[-1] *= -1
B2s = np.dot(R, B2)
# 加速度计的读数
a_s = emz * g0
return np.concatenate((a_s, B1s.reshape(-1), B2s.reshape(-1)))
def stateOut(state, state2, t0, i, mse, printStr, printBool):
'''
输出算法的中间结果
:param state:【np.array】 位置和姿态:x, y, z, q0, q1, q2, q3 (7,)
:param state2: 【np.array】位置、姿态、磁矩、单步耗时和迭代步数 (10,)
:param t0: 【float】 时间戳
:param i: 【int】迭代步数
:param mse: 【float】残差
:return:
'''
if not printBool:
return
print(printStr)
timeCost = (datetime.datetime.now() - t0).total_seconds()
state2[:] = np.concatenate((state, np.array([MOMENT, timeCost,
i]))) # 输出的结果
pos = np.round(state[:3], 3)
R = q2R(state[3:7])
emx = np.round(R[:, 0], 3)
emz = np.round(R[:, -1], 3)
sensor_pred = h0(state)[:6]
print('i={}, pos={}m, q={}, emz={}, sensor_pred={}'.format(
i, pos, np.round(state[3:7], 3), emz, np.round(sensor_pred, 2)))
def sim(sensor_std,
plotType,
state0,
plotBool,
printBool,
state=None,
maxIter=50):
"""
使用模拟的观测值验证算法的准确性
:param state: 【list】模拟的真实状态，可以有多个不同的状态
:param sensor_std: 【float】sensor的噪声标准差[mG]
:param plotType: 【tuple】描绘位置的分量 'xy' or 'yz'
:param plotBool: 【bool】是否绘图
:param printBool: 【bool】是否打印输出
:param maxIter: 【int】最大迭代次数
:return: 【tuple】 位置[x, y, z]和姿态ez的误差百分比
"""
if state is None:
# state = [0, 0.2, 0.4, 0, 0, 1, 1]
state = [0, 0.2, 0.4, 0.1, 0, 0, 0, 0, 1, 1]
mp = Tracker(sensor_std, state0, state)
E = np.zeros(mp.measureNum)
Esim = np.zeros((mp.measureNum, maxIter))
useSaved = False
if useSaved:
f = open('Esim.json', 'r')
simData = json.load(f)
for j in range(mp.measureNum):
for k in range(maxIter):
Esim[j, k] = simData.get('Esim{}-{}'.format(j, k), 0)
print('++++read saved Esim data+++')
else:
std = sensor_std
# em1Sim = q2R(state[3: 7])[:, -1]
# dArray = state[:3] - mp.coilArray
# for i, d in enumerate(dArray):
# # E[i * 3] = inducedVolatage(d=d, em1=(1, 0, 0), em2=em1Sim) # x线圈阵列产生的感应电压中间值
# # E[i * 3 + 1] = inducedVolatage(d=d, em1=(0, 1, 0), em2=em1Sim) # y线圈阵列产生的感应电压中间值
# # E[i * 3 + 2] = inducedVolatage(d=d, em1=(0, 0, 1), em2=em1Sim) # z线圈阵列产生的感应电压中间值
# E[i] = inducedVolatage(d=d, em2=em1Sim) # 单向线圈阵列产生的感应电压中间值
E = mp.h(state)
simData = {}
for j in range(mp.measureNum):
Esim[j, :] = np.random.normal(E[j], std, maxIter)
# plt.hist(Esim[j, :], bins=25, histtype='bar', rwidth=2)
# plt.show()
for k in range(maxIter):
simData['Esim{}-{}'.format(j, k)] = Esim[j, k]
# 保存模拟数据到本地
# f = open('Esim.json', 'w')
# json.dump(simData, f, indent=4)
# f.close()
# print('++++save new Esim data+++')
# 运行模拟数据
for i in range(maxIter):
if printBool:
print('=========={}=========='.format(i))
if plotBool:
plt.ion()
plotP(mp, state, i, plotType)
if i == maxIter - 1:
plt.ioff()
plt.show()
posPre = mp.ukf.x[:3]
mp.run(printBool, Esim[:, i])
delta_x = np.linalg.norm(mp.ukf.x[:3] - posPre)
# print('delta_x={:.3e}'.format(delta_x))
if delta_x < 1e-3:
if plotBool:
plt.ioff()
plt.show()
else:
break
err_pos = np.linalg.norm(mp.ukf.x[:3] - state[:3]) / np.linalg.norm(
state[:3])
err_em = np.linalg.norm(
q2R(mp.ukf.x[6:10])[:, -1] - q2R(state[6:10])[:, -1])
print('\nerr_std: pos={}, err_pos={:.0%}, err_em={:.0%}'.format(
np.round(state[:3], 3), err_pos, err_em))
return (err_pos, err_em)