small improvement to sensor fusion

1) there was a typo when computing the system covariance
a term in dT^3 was ommitted; the impact was was very limited
because of how small this term is.

2) initialize the system covariance matrix with non-zero
values for the gyro-bias part. this improves the initial
bias estimation speed significantly.

3) added comments here and there

Change-Id: I4328c9cca73e089889d5e74b9fda99d7831762dc