GRADE Calc - Filtered and Fit

I was just thinking about this myself. I wanted to implement InigoTolosa code, but haven't sat down to do that yet. Joost.van.Hooren, could you maybe post the CIQ class/function that implements this? I know most of it is available from the pseudocode above, but if you've already created the MonkeyC instance, that would be great.

The class works perfectly as posted by InigoTolosa above; just save as e.g. SimpleKalmanFilter.mc file.

How I implemented in my WatchUi.DataField class:

First I initialized all required variables:

// Variables to store 'measured' altitude and elapsedDistance
hidden var altitude = 0;
hidden var elapsedDistance = 0;
hidden var lastElapsedDistance = 0;

// Variables to store 'filtered' altitude and elapsedDistance
hidden var altitudeKalmanFilter;
hidden var distanceKalmanFilter;

// Variables to store gradient (and VAM)
hidden var grade = 0;
hidden var vam;

Then I initialize 2 SimpleKalmanFilter classes, one for altitude and one for distance, within my DataField.initialize() function:

function initialize() {
    DataField.initialize();

    var errMeasure = Properties.getValue("AltErrMeasure");
    var errEstimate = Properties.getValue("AltErrEstimate");
    var maxProcessNoise = Properties.getValue("AltMaxProcessNoise");
    altitudeKalmanFilter = new SimpleKalmanFilter(errMeasure, errEstimate, maxProcessNoise);

    errMeasure = Properties.getValue("DistErrMeasure");
    errEstimate = Properties.getValue("DistErrEstimate");
    maxProcessNoise = Properties.getValue("DistMaxProcessNoise");
    distanceKalmanFilter = new SimpleKalmanFilter(errMeasure, errEstimate, maxProcessNoise);
}

Within my DataField.compute(info) function I calculate gradient (grade) and VAM:

function compute(info) {
	altitude = info.altitude;
	// Active Timer Values
	if (timerState == Activity.TIMER_STATE_ON) {
	    elapsedDistance = (info.elapsedDistance != null) ? (info.elapsedDistance) : (0);
		// Calculate smooth Gradient and VAM, applying Simple Kalman Filter
		if (elapsedDistance != 0) {
    		var lastAltitude = altitudeKalmanFilter.getLastEstimate();
    		var lastDistance = distanceKalmanFilter.getLastEstimate();
    		var currentAltitude = altitudeKalmanFilter.updateEstimate(altitude);
			var currentDistance = distanceKalmanFilter.updateEstimate(elapsedDistance - lastElapsedDistance);
			grade = (currentAltitude - lastAltitude) / currentDistance * 100;
			vam = ((currentAltitude - lastAltitude) * 3600).toNumber();
		}
		lastElapsedDistance = elapsedDistance;
	}
}

Also don't forget to implement DataField.onTimerStart() and DataField.onTimerResume(), as InigoTolosa mentioned: "Take into account that in order to avoid abnormal results during initial seconds, you must set initial state of the filter passing the first reading of the magnitud to be filtered when you run the filter for first time or you resume it after pausing."

function onTimerStart() {
	altitudeKalmanFilter.setInitialState(altitude);
	distanceKalmanFilter.setInitialState(elapsedDistance);
}

function onTimerResume() {
	altitudeKalmanFilter.setInitialState(altitude);
	distanceKalmanFilter.setInitialState(elapsedDistance);
}

Happy riding!

Awesome!! Thank you!! I’ll implement and do some IRL testing.

Ow, and note that I've set the Kalman Filter variables (errMeasure, errEstimate and maxProcessNoise) as properties/settings, so I can change them via Garmin Connect without having to change the app. Let me know if this requires further clarification.

Sounds like I've got some studying to do... catching up on Kalman Filter 

Great idea. As you tweak those variables and arrive at a set that seems optimal, give us an update.

After studying some theory of Kalman Filtering and working on a model (in Excel) to reproduce results and validate filter settings by playing with the variables, I realized that I've incorrectly applied (this simple) Kalman filter to elapsed distance... as a result this roughly reduces distance to half the real distance, hence distorts gradient calculation. I've adapted to apply the filter to distance between 2 measurements instead of total elapsed distance, which seems to produce good results. In a first attempt I've arrived at reasonable results with below. 

For altitude, Error of Estimate and Max Process Noise don't seem to make much of a difference, because they're being recalculated by the code. Error of Measure is the one that helps to optimize results.

For Distance, Error of Estimate and Error of Measure don't really seem to influence the results. For this one it's much more the Max Process Noise that seems to drive results.

See results:

Happy to share updated code and/or model when I have a little more time or when I've further optimized.

The code only requires two little tweaks:

hidden var lastElapsedDistance = 0;

{
    var currentDistance = distanceKalmanFilter.updateEstimate(elapsedDistance - lastElapsedDistance);
    grade = (currentAltitude - lastAltitude) / currentDistance * 100;
}

lastElapsedDistance = elapsedDistance;

Plus of course initializing the variable and setting it to store lastElapsedDistance after calculation is done (I've updated my code above).