hello ,
i want to fuse yaw angle by rtk gps heading instead of mag ? what should i do?
1 Like
anybody?
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.
Others?
Christian
1 Like
You can get very precise heading with a dual GNSS reciever.
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.
1 Like
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.
That’s a good idea, its worth trying.
. 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.
// 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
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
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); _omega_yaw_P.zero(); _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); _omega_yaw_P.zero(); _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; return; }
// 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);}
This is low on my list of priorities. Sorry!
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.
How is your trying? Any good result?
Work have done, it works well.
Hi,
This sounds interesting Liang-Tang.
Can you share how you did it and what accuracy you achieved?
Christian
here is the solution
1 Like
Hi, how is it going?
works well, but another way.
Hi Liang
Do you have any more info on how you get GPS heading to work?
We are trying to use Tersus GPS for heading, but wondering about GPS yaw offset parameters.
Px4 flight stack has this parameter, but I cannot find it in ARDUcopter.
Thanks
You should add the param by yourself. By the way, we use NOVA rtk headingb message to obtain gps heading, because it can offer yaw heading status(2D, 3D, DGPS, float, fixed…), only in fixed status, heading is fused.