function coeffs = fit_current_coeffs(torque, rate, current)
  % Fit current model coefficients to raw data.
  %
  % This function performs a linear least-squares regression to fit a set of
  % polynomial coefficients to the provided data. The fitted polynomial
  % coefficients can be used in a model that calculates motor current draw as
  % a function of motor torque and rack rate. The polynomial basis terms are
  % defined in basis.m
  %
  % The provided data can be arbitrary driving (does not need to consist of
  % specific maneuvers) but do take care to make sure that the data covers the
  % range of inputs (motor torque and rack rate) that you're interested in
  % modelling accurately.
  %
  % It is recommned that you err on the side of providing a large data set
  % (on a conventional laptop, this fitting process and easily support 1.5e6
  % samples). If you provided too little data, you risk over-fitting to a small
  % sample that doesn't generalize well.
  %
  % A max rate is hardcoded to discard any values above this threshold from the
  % fit. Similiarly, a max current is hardcoded to discard any values above
  % this threshold.
  %
  % Note that it is recommend that you include both left and right cornering,
  % although the fitted coefficients will represent a model that is left/right
  % symmetric.
  %
  % Arguments:
  % torque: motor torque (Nm) [Nx1]
  % rate: rack rate (m/s) [Nx1]
  % current: motor current draw (A) [Nx1]
  %
  % Outputs:
  % coeffs: fitted polynomial coefficients
  %

  max_current = 80;
  max_rate = 100/1000;

  % discard -ve power (regen?)
  power = torque.*rate;
  b_filter = (power > 0);
  torque = torque(b_filter);
  rate = rate(b_filter);
  current = current(b_filter);

  % assume data is symmetric
  b_filter = (rate < 0);
  torque(b_filter) = -torque(b_filter);
  rate(b_filter) = -rate(b_filter);

  % discard rate > max_rate
  b_filter = (rate <= max_rate);
  torque = torque(b_filter);
  rate = rate(b_filter);
  current = current(b_filter);

  % discard current > max_current
  b_filter = (current <= max_current);
  torque = torque(b_filter);
  rate = rate(b_filter);
  current = current(b_filter);

  fprintf('Number of samples for regression: %d\n', numel(torque));

  coeffs = basis(torque, rate)\current;

  current_model = basis(torque, rate)*coeffs;

  error = current_model - current;
  sse = sum(error.^2)

  figure(1)
  h(1) = subplot(221);
  plot(torque, 'b')
  xlabel('index')
  ylabel('torque (Nm)')
  grid on

  h(2) = subplot(222);
  plot(current, 'b')
  hold on
  plot(current_model, 'r')
  hold off
  xlabel('index')
  ylabel('current (A)')
  legend({'raw','model'})
  grid on

  h(3) = subplot(223);
  plot(rate*1000, 'b')
  xlabel('index')
  ylabel('rate (mm/s)')
  grid on

  h(4) = subplot(224);
  plot(current_model - current, 'b')
  xlabel('index')
  ylabel('error (A)')
  grid on

  linkaxes(h,'x');

  figure(2)
  plot(current_model, current, '.b')
  hold on
  plot([0, max_current], [0, max_current], 'k', 'linewidth', 2)
  hold off
  axis('equal')
  xlabel('raw (A)')
  ylabel('model (A)')
  grid on
  title('Modelled Current vs. Measured Current')
  legend({'fit', 'perfect correlation'})

  figure(3)
  plot(rate*1000, error, '.')
  title('Current Error vs. Rack Rate')
  xlabel('rate (mm/s)')
  ylabel('current error (A)')
  grid on

  figure(6)
  scatter(torque, current, ones(size(torque)), rate*1000)
  title('Measured Data vs. Model')
  xlabel('Torque (Nm)')
  ylabel('Current (A)')
  cb = colorbar();
  cb.Label.String = 'Rack Rate (mm/s)';
  grid on

  hold on
  torque_0 = linspace(0, 4, 25)';
  rate_0 = 0.0 * ones(size(torque_0));
  current_0 = basis(torque_0, rate_0)*coeffs;
  scatter(torque_0, current_0, 100*ones(size(torque_0)), rate_0*1000, 'filled', 's')
  hold off

  hold on
  torque_0 = linspace(0, 4, 25)';
  rate_0 = 0.05 * ones(size(torque_0));
  current_0 = basis(torque_0, rate_0)*coeffs;
  scatter(torque_0, current_0, 100*ones(size(torque_0)), rate_0*1000, 'filled', 's')
  hold off

  hold on
  torque_0 = linspace(0, 3, 25)';
  rate_0 = max_rate * ones(size(torque_0));
  current_0 = basis(torque_0, rate_0)*coeffs;
  scatter(torque_0, current_0, 100*ones(size(torque_0)), rate_0*1000, 'filled', 's')
  hold off

end

