The attitude and heading reference system (AHRS) is an important concept in the area of navigation, image stabilization, and object detection and tracking. Many studies and works have been conducted in this regard to estimate the accurate orientation of rigid bodies. In most research in this area, low-cost MEMS sensors are employed, but since the system’s response will diverge over time due to integration drift, it is necessary to apply proper estimation algorithms. A two-step extended Kalman Filter (EKF) algorithm is used in this study to estimate the orientation of an IMU. A 9-DOF device is used for this purpose, including a 6-DOF IMU with a three-axis gyroscope and a three-axis accelerometer, and a three-axis magnetometer. In addition, to have an accurate algorithm, both IMU and magnetometer biases and disturbances are modeled and considered in the real-time filter. After applying the algorithm to the sensor’s output, an accurate orientation as well as unbiased angular velocity, linear acceleration, and magnetic field were achieved. In order to demonstrate the reduction of noise power, fast Fourier transform (FFT) diagrams are used. The effect of the initial condition on the response of the system is also investigated.