AP_Compass: Add an index to avoid ergodicity of search in '_samples_collected', w…

[lukezhqin]

Add an index to avoid ergodicity of search in '_samples_collected', which would certainly met the input sample itself to force the distance to zero and return false.

[lukezhqin]

Please help me review this bug fix trial which is concern to issue 12171
#12171 (comment)

[lukezhqin]

Please see here, the state trasition of mag-cal from ...STEP_ONE to ...STEP_TWO:

case COMPASS_CAL_RUNNING_STEP_TWO:
            if(_status != COMPASS_CAL_RUNNING_STEP_ONE) {
                return false;
            }
            thin_samples();
            initialize_fit();
            _status = COMPASS_CAL_RUNNING_STEP_TWO;
            return true;

And then in thin_samples():

void CompassCalibrator::thin_samples() {
    if(_sample_buffer == nullptr) {
        return;
    }

    _samples_thinned = 0;
    // shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle
    // this is so that adjacent samples don't get sequentially eliminated
    for(uint16_t i=_samples_collected-1; i>=1; i--) {
        uint16_t j = get_random16() % (i+1);
        CompassSample temp = _sample_buffer[i];
        _sample_buffer[i] = _sample_buffer[j];
        _sample_buffer[j] = temp;
    }

    for(uint16_t i=0; i < _samples_collected; i++) {
        if(!accept_sample(_sample_buffer[i])) {
            _sample_buffer[i] = _sample_buffer[_samples_collected-1];
            _samples_collected --;
            _samples_thinned ++;
        }
    }

    update_completion_mask();
}

Finally, in accept_sample():

bool CompassCalibrator::accept_sample(const Vector3f& sample)
{
    static const uint16_t faces = (2 * COMPASS_CAL_NUM_SAMPLES - 4);
    static const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
    static const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));

    if(_sample_buffer == nullptr) {
        return false;
    }

    float min_distance = _params.radius * 2*sinf(theta/2);

    for (uint16_t i = 0; i<_samples_collected; i++){
        float distance = (sample - _sample_buffer[i].get()).length();
        if(distance < min_distance) {
            return false;
        }
    }
    return true;
}

You would clearly find out the sample send into accept_sample() would be elimilated by the ergodicity search of distance check in the same total sample buffer which include the sample sent in itself.
I just don't understand that we shall abandon the former 10 steps iteration caculation in ...STEP_ONE when we enter into ...STEP_TWO which doesn't make any sense to me definitely.
Would it just design to be that or like to be a very bug?

[lukezhqin]

@tridge , I looded through all the history to find you committed these part. Would you help me review this PR here?

[rmackay9]

@rmackay9, can you please have a look?

[tridge]

why the extra temporary variable 'temp' ?

[tridge]

could you add a comment to this function explaining what it is doing?

[peterbarker]

This is just a copy of the existing accept_sample which exempts an item from the distance check.

It would better be done with an optional int32_t idx with a magic value of -1 meaning don't exempt any element from the check.

[lukezhqin]

I just made a simple commit which I would thought it could be somewhat a litter clear for you guys to understand what I mean to talked about in my PR. I then would push a better commit to this PR based in @peterbarker mentioned above.

[tridge]

do you have a tlog of the old method failing? I don't quite see how it fails

[peterbarker]

@tridge this definitely looks suspect. When we're thinning samples we pass in an element from the sample buffer into accept_sample - note that we don't pass in anything about which sample we're testing.

In accept_sample, we loop over all of the samples in the sample buffer - and that must include the one we've just passed in (as no index variable have been modifed).

This line here:

        float distance = (sample - _sample_buffer[i].get()).length();

Will always get a distance of zero, accept will then return false. We prune exactly half the samples (the first half in the list).

[rmackay9]

OK, after discussing with @peterbarker we understand the problem now. We will include this in an upcoming PR that cleans up this calibrator.

[lukezhqin]

@peterbarker Thank you for your reply, but I didn't get the query message or e-mail from @tridge from github. Which I shall carefully check my account setting later. @rmackay9 ,Yes, that's exactly what I mean here.
I always thought about our calibration could be so long and boring. So I looked through all the codes details then I found this sample_thin method could have this kind of problem. Thank you for your understanding.

[peterbarker]

@lukezhqin Randy is incorporating this fix into a more elaborate PR here: https://github.com/ArduPilot/ardupilot/pull/12864/files

Many thanks for tracking this one down.

[rmackay9]

If it's OK, I will close this PR because the fix is included in this PR: #12864

[lukezhqin]

@peterbarker @rmackay9 This did bother me a lot in the past days. Many thanks to you guys at last.