function [residual,theta] = smooth(measTheta, thetaVar ,measThetaDot, ...
thetaDotVar, deltaTime, theta0, weight)
%SMOOTH Calculate function and residual for weighted data
%   smooth calculates the theta value at each time step as the weighted
%   average of the measured theta (in our case from the magnetometer) at the
%   time step and the calculated theta using trapezoidal integration from
%   the previous time step to the current one with the measured theta_dot
%   (in our case from the z-axis rate gyro). The residual is the square
%   root of the sum of the weighted square residuals at each time step.
%   The weights are the reciprocals of the measured variances for the
%   magnetometer angle and the rate gyro.
%   Inputs:
%   measTheta -- The unwrapped roll angles calculated from the magnetometer
%   measurements.
%   thetaVar -- The measured variance of the noise from the angle
%   calculations with the magnetometer.
%   measThetaDot -- The roll angle rates as measured by the z-axis rate
%   gyro.
%   thetaDotVar -- The measured variance in the noise of the rate gyro.
%   deltaTime -- The sampling or calculation period, most commonly 0.01.
%   theta0 -- The initial angle of rotation on the pad.
%   weight -- The weighting of the magnetometer angle in the calculation.
%   The weighting of the gyro calculation is 1 - weight. For the J825R file
%   the optimal value for the weight is 0.1
n = length(measTheta); % Length of data arrays
theta(1) = theta0; % Initialize theta
dto2 = deltaTime/2; % Set for trapezoidal calculation
for i = 1:n-1
    % Integrate the rotation rate to the next time step with the
    % trapezoidal rule using the rate gyro data.
    thetaStep = theta(i) + dto2*(measThetaDot(i+1) + measThetaDot(i));
    % Calculate theta as the weighted average.
    theta(i+1) = weight*measTheta(i+1) + (1-weight)*thetaStep;
    % Calculate the weighted residual. We use reciprocals because the
    % larger the variance, the less you want it to count.
    res(i) = (theta(i+1) - measTheta(i+1))^2/thetaVar ...
        + (theta(i+1) - thetaStep)^2/deltaTime/thetaDotVar;
end
residual = sqrt(sum(res));
end

