A method includes providing a Bottom Hole Assembly (BHA) in a wellbore. The BHA includes a rotary steerable system and a downhole attitude correction and control system. The downhole correction and control system includes a first sensor set, the sensors of the first sensor set positioned near ferromagnetic components of a drill string and a second sensor set, the sensors of the second sensor set positioned further from the ferromagnetic components of the drill string than the sensors of the first sensor set. Corrupted data from the first sensor set and reference data from the second sensor set is obtained, the corrupted data including cross-axis magnetometer and accelerometer measurements. The method additionally includes correcting the corrupted sensor data to form corrected sensor measurements and calculating an estimated azimuth from the corrected sensor measurements. The method further includes steering the rotary steerable system based on the estimated azimuth.