Ejemplo n.º 1
0
  def kfupdate(self, dt, rs):
    self.ab  = array((rs.ax, -rs.ay, -rs.az))

    ph = self.phi
    th = self.theta
    P  = self.pmat

    A  = array(((-rs.q*cos(ph)*tan(th)+rs.r*sin(ph)*tan(th), (-rs.q*sin(ph)+rs.r*cos(ph))/(cos(th)*cos(th))),
                (rs.q*sin(ph)-rs.r*cos(ph)                 , 0)))

    dph = rs.p - rs.q*sin(ph)*tan(th) - rs.r*cos(ph)*tan(th)
    dth =      - rs.q*cos(ph)         - rs.r*sin(ph)
    dP  = dot(A, P) + dot(P, transpose(A)) + self.Q

    ph = ph + dph * dt
    th = th + dth * dt
    P  = P  + dP  * dt

    Cx = array((0               , cos(th)))
    Cy = array((-cos(th)*cos(ph), sin(th)*sin(ph)))
    Cz = array((cos(th)*sin(ph) , sin(th)*cos(ph)))
    C  = array((Cx, Cy, Cz))

    L = dot(dot(P, transpose(C)), inverse(self.R + dot(dot(C, P), transpose(C))))
    h = array((sin(th), -cos(th)*sin(ph), -cos(th)*cos(ph)))

    P  = dot(identity(2) - dot(L, C), P)
    ph = ph + dot(L[0], self.ab - h)
    th = th + dot(L[1], self.ab - h) 

    ph = ((ph+pi) % (2*pi)) - pi;
    th = ((th+pi) % (2*pi)) - pi;

    self.pmat  = P
    self.phi   = ph 
    self.theta = th

    psidot = rs.q * sin(ph) / cos(th) + rs.r * cos(ph) / cos(th);
    self.psi += psidot * dt;

    self.quat = eul2quat(ph,th,self.psi)
    # self.dcmb2e = quat2dcm(quatinv(self.quat))

    # self.ae = dot(self.dcmb2e, self.ab)
    self.ae = quatrotate(quatinv(self.quat), self.ab)
    self.ae[2] = -self.ae[2]-1
    self.ae[1] = -self.ae[1]

    self.ve += self.ae * dt * 9.81
    self.xe += self.ve * dt
Ejemplo n.º 2
0
 def location(self, q):
     return -quatrotate(quatinv(q), array((self.rxb, self.ryb, self.rzb)))