def test_batch_equals_recursive():
""" ensures that the batch filter and the recursive version both
produce the same results.
"""
N = 4 # size of lag
fls = FixedLagSmoother(dim_x=2, dim_z=1, N=N)
fls.x = np.array([0., .5])
fls.F = np.array([[1., 1.], [0., 1.]])
fls.H = np.array([[1., 0.]])
fls.P *= 200
fls.R *= 5.
fls.Q *= 0.001
nom = np.array([t / 2. for t in range(0, 40)])
zs = np.array([t + random.randn() * 1.1 for t in nom])
xs, x = fls.smooth_batch(zs, N)
for k, z in enumerate(zs):
fls.smooth(z)
xSmooth = np.asarray(fls.xSmooth)
xfl = xs[:, 0].T[0]
res = xSmooth.T[0, 0] - xfl
assert np.sum(res) < 1.e-12
class Smoother(object):
def __init__(self, N=15):
self.smoother = FixedLagSmoother(dim_x=2, dim_z=1, N=N)
self.smoother.x = np.array([0., .5])
self.smoother.F = np.array([[1., 1.], [0., 1.]])
self.smoother.H = np.array([[1., 0.]])
self.smoother.P *= 1000
self.smoother.R *= 15.
self.smoother.Q *= 0.1
def smooth(self, value):
self.smoother.smooth(value)
def get_smoothed_value(self, value):
self.smooth(value)
return np.array(self.smoother.xSmooth)[:, 0][-1]
def test_batch_equals_recursive():
""" ensures that the batch filter and the recursive version both
produce the same results.
"""
N = 4 # size of lag
fls = FixedLagSmoother(dim_x=2, dim_z=1, N=N)
fls.x = np.array([0., .5])
fls.F = np.array([[1.,1.],
[0.,1.]])
fls.H = np.array([[1.,0.]])
fls.P *= 200
fls.R *= 5.
fls.Q *= 0.001
nom = np.array([t/2. for t in range (0,40)])
zs = np.array([t + random.randn()*1.1 for t in nom])
xs, x = fls.smooth_batch(zs, N)
for k,z in enumerate(zs):
fls.smooth(z)
xSmooth = np.asarray(fls.xSmooth)
xfl = xs[:,0].T[0]
res = xSmooth.T[0,0] - xfl
assert np.sum(res) < 1.e-12
class Smoother():
def __init__(self, initx=0., inity=0.,inith=0):
inith = float(inith)
# xfilter smoothes the movement on the x axis
self.xfilter = FixedLagSmoother(dim_x=2, dim_z=1, N=50)
self.xfilter.x = np.array([[initx], [0.]])
self.xfilter.F = np.array([[1., 1.], [0., 1.]])
self.xfilter.H = np.array([[1., 1]])
self.xfilter.P *= 10 ** 4
self.xfilter.R = 50.0
self.xfilter.Q = Q_discrete_white_noise(2, 1.0, 1.0)
# yfilter smoothes the movement on the y axis
self.yfilter = FixedLagSmoother(dim_x=2, dim_z=1, N=50)
self.yfilter.x = np.array([[inity], [0.]])
self.yfilter.F = np.array([[1., 1.], [0., 1.]])
self.yfilter.H = np.array([[1., 50.]])
self.yfilter.P *= 10.0 ** 4
self.yfilter.R = 50.0
self.yfilter.Q = Q_discrete_white_noise(2, 1.0, 1.0)
# hfilter or heightfilter smoothes out the height changes of the boxes
self.hfilter = FixedLagSmoother(dim_x=2, dim_z=1, N=50)
self.hfilter.x = np.array([[inith],[.5]])
self.hfilter.F = np.array([[1., 1.],[0., 1.]])
self.hfilter.H = np.array([[1., 1.]])
self.hfilter.P *= 10.0**4
self.hfilter.R *= 100.0
self.hfilter.Q *= 0.001
def predict(self, h):
raise Exception("remove, diese Funktion wird nicht benoetig, lag smooth arbeitet nur mit smooth; nicht mit predict und update")
def update(self, x, y, h):
self.xfilter.smooth(x)
self.yfilter.smooth(y)
self.hfilter.smooth(h)
# wenn smooth zu lang kuerzen, benoetigt werden N frames
if len(self.xfilter.xSmooth) > 500: self.xfilter.xSmooth = self.xfilter.xSmooth[-51:-1]
if len(self.yfilter.xSmooth) > 500: self.yfilter.xSmooth = self.yfilter.xSmooth[-51:-1]
if len(self.hfilter.xSmooth) > 500: self.hfilter.xSmooth = self.hfilter.xSmooth[-51:-1]
return int(round(self.xfilter.xSmooth[-1][0][0])), int(round(self.yfilter.xSmooth[-1][0][0])), int(round(self.hfilter.xSmooth[-1][0][0]))
def readSerial(B0, Bs, Bg, slavePlot=0):
port = list(serial.tools.list_ports.comports())[-1][0]
ser = serial.Serial(port,
9600,
timeout=0.5,
parity=serial.PARITY_NONE,
rtscts=1)
navg = 10 # 均值平滑的数据个数
Bnavg = np.zeros((SLAVES, 3)) # 均值平滑储存的数据
nmax = 200 # 为计算标准差采集数据个数
Bnmax = np.zeros((SLAVES, 3, nmax)) # 计算标准差储存的数据
OriginData = np.zeros((SLAVES, 3), dtype=np.float)
magOffsetData = np.zeros((SLAVES, 3), dtype=np.float)
offsetOk = True
n = 0
# 固定区间平滑器
fls = FLS(dim_x=SLAVES * 3, dim_z=SLAVES * 3, N=20)
fls.P *= 200
fls.R *= 50
fls.Q *= 0.5
# 使用闭包来评估数据是否满足正态分布
ftestNormal = testNormal
# 读取本地保存的背景磁场
if offsetOk:
f = open('bg.json', 'r')
bg = json.load(f)
for row in range(SLAVES):
for col in range(3):
Bg[row * 3 + col] = bg.get('B{}{}'.format(row, col), 0)
f.close()
print('get background B OK!')
# 持续读取sensor数据
while True:
if ser.in_waiting:
nn = n % nmax
for slave in range(SLAVES):
[Bx_L, Bx_H, By_L, By_H, Bz_L, Bz_H, id] = ser.read(7)
OriginData[id - 1,
0] = -1.5 * complement2origin((Bx_H << 8) + Bx_L)
OriginData[id - 1,
1] = 1.5 * complement2origin((By_H << 8) + By_L)
OriginData[id - 1,
2] = 1.5 * complement2origin((Bz_H << 8) + Bz_L)
# 扣除背景磁场
if (not offsetOk) and n < 300:
magOffsetData += OriginData
elif (not offsetOk) and n == 300:
Bg[:] = magOffsetData.reshape(-1) // 300
offsetOk = True
print('Calibrate ok!')
# 保存背景磁场到本地json文件
bg = {}
for row in range(SLAVES):
for col in range(3):
bg['B{}{}'.format(row, col)] = Bg[row * 3 + col]
f = open('bg.json', 'w')
json.dump(bg, f, indent=4)
f.close()
else:
OriginData -= np.array(Bg).reshape(9, 3)
B0[:] = np.hstack(OriginData)[:]
# 使用FixedLagSmoother对原始数据进行平滑
fls.smooth(B0[:])
tmp = np.array(fls.xSmooth[0])
Bs[:] = np.array(fls.xSmooth[-1])[0, :]
# 取navg个点的平均值进行平滑
# Bnavg += OriginData
# if n % navg == 0:
# Bs[:] = np.hstack(Bnavg // navg)[:]
# Bnavg = np.zeros((SLAVES, 3))
# Bnmax[slavePlot, 2, (n // navg) % (nmax // navg)] = Bs[14]
# 计算nmax个点的平均值和标准差
# Bnmax[slavePlot, 1, nn] = Bs[slavePlot * 3 + 1]
# if nn == 0 and n > 0:
# ftestNormal(Bnmax[slavePlot, 1])
# Bnmax = np.zeros((SLAVES, 3, nmax))
n += 1
def applyKalmanFilter(csv_file, kalman_file):
# Read in csv file into a seperate dataframe
csvFile = pd.read_csv(csv_file, header=None, dtype=np.float64)
d = csvFile[1].values
fls = FixedLagSmoother(dim_x=2, dim_z=1, N=8)
fls.x = np.array([0., .5])
fls.F = np.array([[1.,1.],
[0.,1.]])
fls.H = np.array([[1.,0.]])
fls.P *= 200
fls.R *= 5.
fls.Q *= 0.001
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([0., .5])
kf.F = np.array([[1.,1.],
[0.,1.]])
kf.H = np.array([[1.,0.]])
kf.P *= 200
kf.R *= 1
kf.Q *= 0.0002
N = 4 # size of lag
#set zs equal to dataframe variable
zs = d
nom = np.array([t/2. for t in range (0, len(zs))])
for z in zs:
fls.smooth(z)
kf_x, _, _, _ = kf.batch_filter(zs)
x_smooth = np.array(fls.xSmooth)[:, 0]
fls_res = abs(x_smooth - nom)
kf_res = abs(kf_x[:, 0] - nom)
plt.plot(zs,'o', alpha=0.5, marker='o', label='zs')
plt.plot(x_smooth, label='FLS')
plt.plot(kf_x[:, 0], label='KF', ls='--')
plt.legend(loc=4)
# print('standard deviation fixed-lag: {:.3f}'.format(np.mean(fls_res)))
# print('standard deviation kalman: {:.3f}'.format(np.mean(kf_res)))
# print(x_smooth[:])#input frame value to print smoothed x val at that point
#----
zs = zs.reshape((len(zs), 1))
zs = pd.DataFrame(zs, columns = ['Original'])
#putting smoothed values (array x_smooth) into a DF
smoothedVals = pd.DataFrame(x_smooth[:], columns = ['Smoothed'])
#---
with open (kalman_file, 'w') as csvfile:
writer = csv.writer(csvfile, lineterminator = '\n', delimiter=' ')
for num in x_smooth:
writer.writerow([num])
class ReadData:
# 验证字段
UAVTALK_SYNC_VAL = 0x3c
UAVTALK_TYPE_MASK = 0x78
UAVTALK_TYPE_VER = 0x20
UAV_OBJ_SENSOR = 0x5F9FFBCA
# 重力加速度【m/s^2】
CONST_g0 = 9.8
# AKMsensor的灵敏度【mGs/LSB】
magSensitivity = 0.031
def __init__(self, snesorDict):
# 串口端口号
port = list(serial.tools.list_ports.comports())[0][0]
self.ser = serial.Serial(port, 230400, timeout=0.5)
if self.ser.isOpen():
print("open {} success!\n".format(port))
else:
raise RuntimeError("open failed")
# 传感器种类
self.sensorDict = snesorDict
self.outDataNum = len(snesorDict) * 6
# 存储所有sensor的所有输出，用于计算标准差std
self.sensorAll = []
for sensor_i in range(self.outDataNum):
self.sensorAll.append(Queue())
# 读取的数据
self.imuSensorData = np.zeros((6, 4), dtype='float32')
self.magSensorData = np.zeros((6, 4))
self.timedata = np.zeros(4, dtype='uint32')
# 用于计算原始数据的sigma
self.sensorDataSigma = np.zeros((self.outDataNum, 4), dtype='float32')
# 扣除背景磁场时的计数
self.n = 0
# 是否读取本地保存的背景磁场
self.offset = True
# 用于平滑磁传感器的数据
self.magSmooth = np.zeros((6, 4), dtype='float32')
# 固定区间平滑器,暂时只对磁传感器进行平滑
self.fls = FLS(dim_x=6, dim_z=6, N=4)
self.fls.P = 1
self.fls.R = 0.05
self.fls.Q = 0.01
def PIOS_CRC_updateByte(self, crc, data) :
crc_table = [
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d,
0xe0, 0xe7, 0xee, 0xe9, 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, 0xb4, 0xb3, 0xba, 0xbd,
0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea,
0xb7, 0xb0, 0xb9, 0xbe, 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0d, 0x0a,
0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a,
0x89, 0x8e, 0x87, 0x80, 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, 0xdd, 0xda, 0xd3, 0xd4,
0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44,
0x19, 0x1e, 0x17, 0x10, 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, 0x6a, 0x6d, 0x64, 0x63,
0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13,
0xae, 0xa9, 0xa0, 0xa7, 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, 0xfa, 0xfd, 0xf4, 0xf3 ]
return crc_table[crc ^ data]
def crc8Calculate(self, curCrc, data):
val = curCrc
for i in range(len(data)) :
val = self.PIOS_CRC_updateByte(val, data[i])
return val
def sensorUnpack(self, data, outputData, outputDataSmooth, magBg, outputDataSigma):
'''
对读到的原始数据进行解包处理
:param data:
:return:
'''
objId = int.from_bytes(data[0:4], 'little')
# print(objId)
if objId == self.UAV_OBJ_SENSOR :
instId = int.from_bytes(data[4:6], 'little')
for i in range(4) :
# 加速度计换算后的单位为[m/s^2]
self.imuSensorData[0, i] = np.asarray(struct.unpack('<f', data[6+i*4:10+i*4])) * 0.001 * self.CONST_g0
self.imuSensorData[1, i] = np.asarray(struct.unpack('<f', data[22+i*4:26+i*4])) * 0.001 * self.CONST_g0
self.imuSensorData[2, i] = np.asarray(struct.unpack('<f', data[38+i*4:42+i*4])) * 0.001 * self.CONST_g0
# 陀螺仪输出的单位为[deg/s]
self.imuSensorData[3, i] = np.asarray(struct.unpack('<f', data[54+i*4:58+i*4]))
self.imuSensorData[4, i] = np.asarray(struct.unpack('<f', data[70+i*4:74+i*4]))
self.imuSensorData[5, i] = np.asarray(struct.unpack('<f', data[86+i*4:90+i*4]))
# AKM磁传感器换算后的单位为[Gs]
for j in range(6):
self.magSensorData[j, i] = np.asarray(
struct.unpack('<h', data[102 + 8*j + 2*i: 104 + 8*j + 2*i])) * self.magSensitivity
# 时间戳
self.timedata[i] = np.asarray(struct.unpack('<i', data[150+i*4:154+i*4]))
# 存储所有sensor的所有输出，用于计算标准差std
if outputDataSigma:
if self.n > 100:
for sensor_i in range(self.outDataNum):
self.sensorAll[sensor_i].get()
for sensor_i in range(6):
if 'imu' not in self.sensorDict.keys():
self.sensorAll[sensor_i].put(self.magSensorData[sensor_i, i])
self.sensorDataSigma[sensor_i][i] = np.array(self.sensorAll[sensor_i].queue).std()
elif 'magSensor' not in self.sensorDict.keys():
self.sensorAll[sensor_i].put(self.imuSensorData[sensor_i, i])
self.sensorDataSigma[sensor_i][i] = np.array(self.sensorAll[sensor_i].queue).std()
else:
self.sensorAll[sensor_i].put(self.imuSensorData[sensor_i, i])
self.sensorAll[sensor_i + 6].put(self.magSensorData[sensor_i, i])
self.sensorDataSigma[sensor_i][i] = np.array(self.sensorAll[sensor_i].queue).std()
self.sensorDataSigma[sensor_i + 6][i] = np.array(self.sensorAll[sensor_i + 6].queue).std()
# 对磁传感器的读数进行平滑
self.fls.smooth(self.magSensorData[:, i])
self.magSmooth[:, i] = np.array(self.fls.xSmooth[-1])[0, :]
if (not self.offset) and self.n < 25:
for i in range(6):
magBg[i] =self.magSensorData[i].sum() + magBg[i]
elif (not self.offset) and self.n == 25:
for i in range(6):
magBg[i] = magBg[i] / self.n / 4
self.offset = True
print('Calibrate magnetic filed ok!')
# 保存背景磁场到本地json文件
bg = {}
for row in range(6):
bg['B{}'.format(row)] = magBg[row]
f = open('bg.json', 'w')
json.dump(bg, f, indent=4)
f.close()
else:
for i in range(6):
self.magSensorData[i] = self.magSensorData[i] - magBg[i]
self.magSmooth[i] = self.magSmooth[i] - magBg[i]
if 'magSensor' not in self.sensorDict.keys():
outputData[:] = np.hstack(np.stack(self.imuSensorData, axis=1))
elif 'imu' not in self.sensorDict.keys():
outputData[:] = np.hstack(np.stack(self.magSensorData, axis=1))
if outputDataSmooth:
outputDataSmooth[:] = np.hstack(np.stack(self.magSmooth, axis=1))
else:
outputData[:] = np.hstack(np.stack(np.vstack((self.imuSensorData, self.magSensorData)), axis=1))
if outputDataSmooth:
outputDataSmooth[:] = np.hstack(np.stack(self.magSmooth, axis=1))
if outputDataSigma:
outputDataSigma[:] = np.hstack(np.stack(self.sensorDataSigma, axis=1))
# print("outputData={}".format(np.round(outputData, 2)))
# print("outputDataSmooth={}".format(np.round(outputDataSmooth, 2)))
def receive(self, outputData, outputDataSmooth, magBg, outputDataSigma=None):
'''
读串口
:param outputDataSigma: 输出数据的标准差
:return:
'''
# Wait a second to let the port initialize
time.sleep(0.01)
viodFlag = b''
# 读取本地保存的背景磁场
if self.offset:
f = open('bg.json', 'r')
bg = json.load(f)
for row in range(6):
magBg[row] = bg.get('B{}'.format(row), 0)
f.close()
print('get background B OK!')
else:
print('start calibrate magnetic field--------------')
while True:
data = self.ser.read() # read data from serial_port
if len(data) > 0 :
syncVal = int.from_bytes(data,'little') # 将字节串转换为整数（反序）
if syncVal == self.UAVTALK_SYNC_VAL : # 验证UAV
crc8 = self.PIOS_CRC_updateByte(0, syncVal)
dataType = int.from_bytes(self.ser.read(),'little')
if dataType == self.UAVTALK_TYPE_VER: # 验证数据类型
crc8 = self.PIOS_CRC_updateByte(crc8, dataType)
dataLen = int.from_bytes(self.ser.read(2),'little')
_dataLen = ctypes.c_short(dataLen)
high_8 = (_dataLen.value & 0xff00) >> 8
crc8 = self.PIOS_CRC_updateByte(crc8, high_8)
low_8 = (_dataLen.value & 0x00ff)
crc8 = self.PIOS_CRC_updateByte(crc8, low_8)
readLen = dataLen + 4 + 2 + 1 # 有用数据的长度
dataBuff = self.ser.read(readLen) # 读取有用数据
objId = int.from_bytes(dataBuff[0:4], 'little')
if len(dataBuff) > 0 and objId == self.UAV_OBJ_SENSOR: # 验证sensor obj id的UAV
crc8 = self.crc8Calculate(crc8, dataBuff)
crc8Val = dataBuff[-1]
decData = binascii.b2a_hex(dataBuff).decode('utf-8')
self.sensorUnpack(dataBuff, outputData, outputDataSmooth, magBg, outputDataSigma)
self.n += 1
def send(self):
'''
向串口发命令
:param serial_port: 串口端口号
:return:
'''
time.sleep(0.5)
initCmd1 = '< J.L0?..rf allinit 23...................................................??'
setsCmd = '< J.L0?..sets capsule 0x96761133 0x0DB7CE15..............................8.'
# initCmd = initCmd1.strip('\n')
cmdBuf = initCmd1.encode('utf-8')
initStr = '3C 20 4A 00 4C 30 D5 02 00 00 72 66 20 61 6C 6C 69 6E 69 74 20 32 33 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 B1'
initBytes = binascii.a2b_hex(initStr.replace(' ', ''))
setsStr = '3C 20 4A 00 4C 30 D5 02 00 00 73 65 74 73 20 63 61 70 73 75 6C 65 20 30 78 39 36 37 36 31 31 33 33 20 30 78 30 44 42 37 43 45 31 35 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 38'
setsBytes = binascii.a2b_hex(setsStr.replace(' ', ''))
self.ser.write(initBytes)
print(initBytes)
time.sleep(1)
self.ser.write(setsBytes)
print(setsBytes)
time.sleep(1)