Disclosed is a system and method for onboard optimal estimation of heading, pitch, and roll through real-time measurement of magnetic field, acceleration and angular motion in three dimensions. Magnetometer information is used to create an initial reference from which movement is measured. Thus, the process does not have to start when the body is in a known position. Further, the device does not have to continually rely on accelerometer data to get roll and pitch. To do this magnetic field data is used to complement gyro information. The magnetic data is used to estimate pitch, roll, and heading.