function [data] = kalman4(datafile)
// Kalman filter for tilt.
//
// This filter uses measurements of tilt angle and tilt rate. It is a response
// to the flawed filter at rotomotion.com.
//
// System states: tilt, tilt rate, tilt acceleration, tilt rate bias

x=[0;
   0;
   0;
   0];
dt = 1/35;                      // System sample rate
A=[ 1 dt dt*dt/2 0;             // State transtion matrix
    0 1  dt      0;
    0 0   1      0;
    0 0   0      1];
H=[ 1 0 0 0;          // measurement matrix
    0 1 0 1];
Q=[0 0 0 0;           // Process noise
   0 0 0 0;
   0 0 1/10000 0;
   0 0 0 0];
R=[.001 0;  // Measurement noise
    0 0.75];
P=[1 0 0 0;
   0 1 0 0;
   0 0 1 0;
   0 0 0 1];    // Initialize  covariance matrix
I = eye(4,4);

// The gains should converge within 1,000 iterations or so.
for i = 1:1000
  P = A*P*A' + Q;          // Covariance update
  K = P*H'*inv(H*P*H' + R); // Kalman gain
  P = (I - K*H)*P;         // Covariance correction
end

disp(P);
disp(K);

fid=mopen(datafile, 'r')
if(fid == -1)
  error('cannot open datafile');
end


num_lines= 0;
done = 0;
while (done == 0)

  [num_read, time, r_rate, p_rate, y_rate, x_accel, y_accel, z_accel, roll, pitch, yaw] = mfscanf(fid, "%f %f %f %f %f %f %f %f %f %f");

  if(num_read < 0)            // end of file
    break;
  end

  if(num_read == 10)
    z = [roll; r_rate*180/%pi];
    x = A*x;
    x = x + K*(z - H*x);

    num_lines = num_lines + 1;
    data(num_lines, 1) = time;
    data(num_lines, 2) = roll;
    data(num_lines, 3) = r_rate*180/%pi;
    data(num_lines, 4) = x(1);
    data(num_lines, 5) = x(2);
    data(num_lines, 6) = x(3);
    data(num_lines, 7) = x(4);
  end
end

mclose(fid);


plot(data(:, 1), data(:, 4), '-');

endfunction
