function c = customFcn(params, sensorData, groundTruth)
% Custom Cost function for optimizing the ahrsfilter

% Set any nontunable parameters in the constructor
decim = 10;
h = ahrsfilter('SampleRate', 200, 'DecimationFactor', decim);

% Parameterize the filter instance with the current-best parameters from
% the params struct.
h.AccelerometerNoise = params.AccelerometerNoise;
h.GyroscopeNoise = params.GyroscopeNoise;
h.MagnetometerNoise = params.MagnetometerNoise;
h.GyroscopeDriftNoise = params.GyroscopeDriftNoise;
h.LinearAccelerationNoise = params.LinearAccelerationNoise;
h.MagneticDisturbanceNoise = params.MagneticDisturbanceNoise;
h.LinearAccelerationDecayFactor = params.LinearAccelerationDecayFactor;
h.MagneticDisturbanceDecayFactor = params.MagneticDisturbanceDecayFactor;
h.ExpectedMagneticFieldStrength = params.ExpectedMagneticFieldStrength;

% Fuse sensor data
qest = h(sensorData.Accelerometer, sensorData.Gyroscope, ...
    sensorData.Magnetometer);

% Compute the orientation error
d = dist(qest, groundTruth.Orientation(1:decim:end));

% Penalize outliers heavily by using the 6th power.
c = sqrt(sqrt( mean(d(10:end,:).^6) ));