def __init__(self, s0, q0, points0, w0, v0, p, q, r):
EKF.__init__(self, 3*M+13, 3*M, pval=p, qval=q, rval=r)
#初始状态估计
self.x[0][0] = s0[0]
self.x[1][0] = s0[1]
self.x[2][0] = s0[2]
self.x[3][0] = v0[0]
self.x[4][0] = v0[1]
self.x[5][0] = v0[2]
self.x[6][0] = q0[0]
self.x[7][0] = q0[1]
self.x[8][0] = q0[2]
self.x[9][0] = q0[3]
self.x[10][0] = w0[0]
self.x[11][0] = w0[1]
self.x[12][0] = w0[2]
for i in range(0, M):
self.x[3*i+13][0] = points0[i][0]
self.x[3*i+14][0] = points0[i][1]
self.x[3*i+15][0] = points0[i][2]
print("s0:")
print(self.x)
def __init__(self,
n,
m,
s0,
q0,
points0,
w0,
v0,
p,
q,
r,
pre=None,
ifCre=True):
EKF.__init__(self, n, m, pval=p, qval=q, rval=r)
#初始状态估计
if ifCre:
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]
else:
self.x = pre[:-3]
print("s0:")
print(self.x)
def __init__(self):
EKF.__init__(self, 2, 2, pval=1, qval=0.001, rval=0.1)