Servers by jDrones

How to use rtk gps heading?

(Liang Tang) #1

hello ,
i want to fuse yaw angle by rtk gps heading instead of mag ? what should i do?

(Liang Tang) #2


(Christian Labeck) #3

Hi Liang.Tang,

I am not an expert in RTK GPS by any means, but fundamentally if I understand it right GPS (or RTK GPS) helps you with position estimation (RTK GPS in the cm accuracy range, normal GPS m range). But neither of the two would give you a heading, i.e. replace the compass completely.

Having RTK GPS will improve the position estimation a lot so the position estimator has less work to do to extrapolate th position from last known position, acellerometers and compass orientation.

But all I have read so far does not sound as if you can do without a compass completely for accurate orientation information.



(David) #4

You can get very precise heading with a dual GNSS reciever.

(Liang Tang) #5

Hi, Christian Labeck
i mean DUAL ANTENNA RTK GPS, a rtk module has two antennas, just like novatel OEM617D (OEM617D Product Sheet), it can provide accurate heading as below:
ALIGN Heading Accuracy Baseline Accuracy (RMS) 2 m 0.08 deg 4 m 0.05 deg
(by the way, more and more rtk module support outputting heading data).
Because compass is more sensitive, i just want to use it as backup, the primary always is RTK heading in most situation.
My question is how to use rtk heading as primary heading.

(Liang Tang) #6

@priseborough any idea? :slight_smile:

(David) #7

We are interested in doing this as well. My initial thought was to decode the compass data steam, emulate it with RTK heading and push that to the compass port on the pixhawk. Not sure how feasible this approach is.

(Liang Tang) #8

That’s a good idea, its worth trying.:slight_smile:. My thought is that modify this function AP_AHRS_DCM::drift_correction_yaw(void), diasble all compass, modify some code here, try to use gps heading(here is ground_course_cd). Next week, maybe have a try.:grin:

// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
bool new_value = false;
float yaw_error;
float yaw_deltat;

if (AP_AHRS_DCM::use_compass()) {
      we are using compass for yaw
    if (_compass->last_update_usec() != _compass_last_update) {
        yaw_deltat = (_compass->last_update_usec() - _compass_last_update) * 1.0e-6f;
        _compass_last_update = _compass->last_update_usec();
        // we force an additional compass read()
        // here. This has the effect of throwing away
        // the first compass value, which can be bad
        if (!_flags.have_initial_yaw && _compass->read()) {
            float heading = _compass->calculate_heading(_dcm_matrix);
            _dcm_matrix.from_euler(roll, pitch, heading);
            _flags.have_initial_yaw = true;
        new_value = true;
        yaw_error = yaw_error_compass();
        // also update the _gps_last_update, so if we later
        // disable the compass due to significant yaw error we
        // don't suddenly change yaw with a reset
        _gps_last_update = _gps.last_fix_time_ms();
} else if (_flags.fly_forward && have_gps()) {
      we are using GPS for yaw
    if (_gps.last_fix_time_ms() != _gps_last_update &&
            _gps.ground_speed() >= GPS_SPEED_MIN) {
        yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
        _gps_last_update = _gps.last_fix_time_ms();
        new_value = true;
        float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f);
        float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
        yaw_error = sinf(yaw_error_rad);
        /* reset yaw to match GPS heading under any of the
           following 3 conditions:
           1) if we have reached GPS_SPEED_MIN and have never had
           yaw information before
           2) if the last time we got yaw information from the GPS
           is more than 20 seconds ago, which means we may have
           suffered from considerable gyro drift
           3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
           and our yaw error is over 60 degrees, which means very
           poor yaw. This can happen on bungee launch when the
           operator pulls back the plane rapidly enough then on
           release the GPS heading changes very rapidly
        if (!_flags.have_initial_yaw ||
                yaw_deltat > 20 ||
                (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
            // reset DCM matrix based on current yaw
            _dcm_matrix.from_euler(roll, pitch, gps_course_rad);
            _flags.have_initial_yaw = true;
            yaw_error = 0;
if (!new_value) {
    // we don't have any new yaw information
    // slowly decay _omega_yaw_P to cope with loss
    // of our yaw source
    _omega_yaw_P *= 0.97f;
// convert the error vector to body frame
float error_z = _dcm_matrix.c.z * yaw_error;
// the spin rate changes the P gain, and disables the
// integration at higher rates
float spin_rate = _omega.length();
// sanity check _kp_yaw
if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
    _kp_yaw = AP_AHRS_YAW_P_MIN;
// update the proportional control to drag the
// yaw back to the right value. We use a gain
// that depends on the spin rate. See the fastRotations.pdf
// paper from Bill Premerlani
// We also adjust the gain depending on the rate of change of horizontal velocity which
// is proportional to how observable the heading is from the acceerations and GPS velocity
// The accelration derived heading will be more reliable in turns than compass or GPS
_omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain();
if (use_fast_gains()) {
    _omega_yaw_P.z *= 8;
// don't update the drift term if we lost the yaw reference
// for more than 2 seconds
if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) {
    // also add to the I term
    _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
_error_yaw = 0.8f * _error_yaw + 0.2f * fabsf(yaw_error);


(Liang Tang) #9

@skyveyor how’s going?

(David) #10

This is low on my list of priorities. Sorry!

(Nick Zhu) #11

There is dual-antenna GNSS for heading. You need to have two antennas. The accuracy can be (0.2/R) degree. R means the baseline bwteen 2 antennas.

(Nick Zhu) #12

How is your trying? Any good result?

(Liang Tang) #13

Work have done, it works well.

(Christian Labeck) #14


This sounds interesting Liang-Tang.

Can you share how you did it and what accuracy you achieved?


(Liang Tang) #15

here is the solution