def estimate(self, Vair, theta, psi, GPS_Pn, GPS_Pe, dt):
        # CHECK UNITS
        psi = wrap(TD.HEADING)
        theta = TD.PITCH
        Vair = TD.TRUEAIRSPEED * 1.68780986 # m/s to ft/s
        Qpn = (self.Qot*abs(cos(psi))) + (self.Qxt*abs(sin(psi)))
        Qpe = (self.Qot*abs(sin(psi))) + (self.Qxt*abs(cos(psi)))
        Qwn = .00001
        Qwe = .00001
        self.Q = matrix("%f,0,0,0; 0,%f,0,0; 0,0,%f,0; 0,0,0,%f" % (Qpn, Qpe, Qwn, Qwe))

        X_dot = matrix("%f;%f;%f;%f" % (Vair*cos(psi)*cos(theta) - self.Wn_est,
                                        Vair*sin(psi)*cos(theta) - self.We_est,
                                        0,
                                        0))
        self.X = self.X+X_dot*dt

        self.A = matrix("0,0,-1,0;0,0,0,-1;0,0,0,0;0,0,0,0")

        P_dot = self.A*self.P + self.P*self.A.transpose() + self.Q
        self.P = self.P + P_dot*dt

        self.C = matrix("1,0,0,0;0,1,0,0")

        self.L = self.P*self.C.transpose()*linalg.inv(self.R+self.C*self.P*self.C.transpose())
        #To invert the matrix it must not be singular ie have a det()=0

        self.P = (self.I-self.L*self.C)*self.P

        z = matrix("%f;%f" % (GPS_Pn,GPS_Pe))
        h = matrix("%f;%f" % (self.X[0,0],self.X[1,0]))

        self.X = self.X + self.L*(z-h)
        self.Wn_est, self.We_est = self.X[2,0], self.X[3,0]
        return self.X