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)