Golden Cheetah

function CT = KFModel(HR,CTstart)
%Inputs:
%HR = A vector of minute to minute HR values.
%CTstart = Core Body Temperature at time 0.
%Outputs:
%CT = A vector of minute to minute CT estimates
%Extended Kalman Filter Parameters
a = 1; gamma = 0.022^2;
b_0 = -7887.1; b_1 = 384.4286; b_2 = -4.5714; sigma = 18.88^2;
%Initialize Kalman filter
x = CTstart; v = 0;%v = 0 assumes confidence with start value.
%Iterate through HR time sequence
for time = 1:length(HR)
%Time Update Phase
x_pred = a∗x; %Equation 3
v_pred = (a^2)∗v+gamma; %Equation 4
%Observation Update Phase
z = HR(time);
c_vc = 2.∗b_2.∗x_pred+b_1; %Equation 5
k = (v_pred.∗c_vc)./((c_vc.^2).∗v_pred+sigma); %Equation 6
x = x_pred+k.∗(z-(b_2.∗(x_pred.^2)+b_1.∗x_pred+b_0)); %Equation 7
v = (1-k.∗c_vc).∗v_pred; %Equation 8
CT(time) = x;
end

        // This code is based upon the matlab function provided as
        // part of the 2013 paper cited above, bear in mind that the
        // input is HR in minute by minute samples NOT seconds.
        //
        // Props to Andy Froncioni for helping to evaluate this code.
        //
        // function CT = KFModel(HR,CTstart)
        // %Inputs:
        //   %HR = A vector of minute to minute HR values.
        //   %CTstart = Core Body Temperature at time 0.
        //
        // %Outputs:
        //   %CT = A vector of minute to minute CT estimates
        // 
        // %Extended Kalman Filter Parameters
        //   a = 1; gamma = 0.022^2;
        //   b0 = -7887.1; b1 = 384.4286; b2 = -4.5714; sigma = 18.88^2;
        static const double CTStart = 37.0f;
        static const double a1 = 1.0f;
        static const double gamma = 0.022f * 0.022f;
        static const double b0 = -7887.1f; 
        static const double b1 = 384.4286f; 
        static const double b2 = -4.5714f; 
        static const double sigma = 18.88f * 18.88f;
        //
        // %Initialize Kalman filter
        //   x = CTstart; v = 0;            %v = 0 assumes confidence with start value.
        double x = CTStart;
        double v = 0;
        //
        // %Iterate through HR time sequence
        //   for time = 1:length(HR)
        //     %Time Update Phase
        //     x_pred = a ∗ x;                                         %Equation 3
        //     v_pred = (a^2) ∗ v+gamma;                               %Equation 4
        //
        //     %Observation Update Phase
        //     z = HR(time);
        //     c_vc = 2 ∗  b2 ∗ x_pred+b1;                             %Equation 5
        //     k = (v_pred ∗ c_vc)./((c_vc^2) ∗ v_pred+sigma);         %Equation 6
        //     x = x_pred+k ∗ (z-(b2 ∗ (x_pred^2)+b1 ∗ x_pred+b0));    %Equation 7
        //     v = (1-k ∗ c_vc) ∗ v_pred;                              %Equation 8
        //     CT(time) = x;
        // end

        // now compute CT using the algorithm provided
        QVector<double> ctArray(hrArray.size());

        for(int i=0; i<hrArray.count(); i++) {
            double x_pred = a1 * x;
            double v_pred = (a1  * a1 ) * (v+gamma);

            double z = hrArray[i];
            double c_vc = 2.0f *  b2 * x_pred + b1;
            double k = (v_pred * c_vc)/((c_vc*c_vc) * v_pred+sigma);

            x = x_pred+k * (z-(b2 * (x_pred*x_pred)+b1 * x_pred+b0));
            v = (1-k * c_vc) * v_pred;

            ctArray[i] = x;
        }

        // now update the RideFile data points, but only if we got
        // any data i.e. ride is longer than a minute long!
        if (ctArray.count()) {
            int index=0;
            foreach(RideFilePoint *p, dataPoints_) {

                // move on to the next one
                if (double(index)*60.0f < p->secs && index < (ctArray.count()-1)) index++;

                // just use the current value first for index=0 and p->secs=0
                p->tcore = ctArray[index];

                // smooth the values
                //if (index && p->secs > 0 && p->secs <= (double(index)*60.0f)) {
                    //double pt = (p->secs - (double(index-1)*60.00f)) / 60.0f;
                    //p->tcore = (ctArray[index] - ctArray[index-1]) * pt;
                //}
            }
        } else {

            // just set to the starting body temperature for every point
            foreach(RideFilePoint *p, dataPoints_) p->tcore = CTStart;
        }
    }

    // Averages and Totals
    avgPoint->np = NPcount ? (NPtotal / NPcount) : 0;
    totalPoint->np = NPtotal;

    avgPoint->xp = XPcount ? (XPtotal / XPcount) : 0;
    totalPoint->xp = XPtotal;

    avgPoint->apower = APcount ? (APtotal / APcount) : 0;
    totalPoint->apower = APtotal;