The integrated INS/magnetometer measurement is widely used in low-cost navigation systems. The integration has proven more effective in suppressing the divergence of heading than relying solely on a magnetometer because this is susceptible to local magnetic field interference, reducing heading accuracy. Magnetometers sense the local magnetic field that may be interfered by the nearby ferromagnetic material or strong electric currents. Hence, the magnetometer must be calibrated in the vehicle before use. When a magnetometer is installed near power components (engines, etc.), soft iron interference can be ignored. In the vehicle’s external environment, the time-varying hard iron interference can reach 100 times the strength of the geomagnetic field, meaning that a magnetometer cannot function efficiently because its accuracy is so reduced. Hence, the constant hard magnetic interference inside the vehicle is mainly concerned in this paper. An INS/Magnetometer heading estimation algorithm based on a two-stage Kalman filter is proposed to solve the problem by combining inertial sensor and magnetometer with attitude information. In the first stage filter, the constant hard iron interference is estimated by setting upward standing the three IMU axes. In the second stage filter, the INS/Magnetometer heading estimation is implemented. Finally, the results show that the algorithm improves the accuracy of vehicle heading calculations.