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
def location(self, q):
return -quatrotate(quatinv(q), array((self.rxb, self.ryb, self.rzb)))