[Autopilot] kalman filter for lineal position with accelerometer, from modified rotomotion code

[ego <ego...@gm...>]

Hello to all,
I'm new here, and already need your help!!

I'm using a kalman filter to find out the linear position of a moving object
(movement limited to a single axis), where I use a ADX202 accelerometer to
measure the acceleration, and take some noisy measurements of the position
as well.

I'm adapting the rotomotion code for a tilt sensor, where tilt is measured
with a 2 axis accelerometer and angular rate with a gyro. I guess this code
is well known but I post the link here:
http://scratchpad.wikia.com/wiki/RotomotionCode

In my project the state vector would be:

X=[position, velocity, acceleration_bias]

And so the state functions:

Xdot=[position_dot, velocity_dot, acceleration_bias_dot]

position_dot=velocity
velocity_dot= acceleration-acceleration_bias_dot
acceleration_bias_dot=0

And the covariance matrix (initial state):

P=[1 0 0]
[0 0 0]
[0 0 1]

The jacobian of Xdot with respect to the states:

A=[d(pos_dot)/d(position) d(pos_dot)/d(vel) d(pos_dot)/d(a_bias)]
[d(vel_dot)/d(position) d(vel_dot)/d(vel) d(vel_dot)/d(a_bias)]
[d(a_bias_dot/d(pos) d(a_bias_dot/d(vel) d(a_bias_dot/d(a_bias)]

A=[0 1 -dt]
[0 0 -1]
[0 0 0] (is this correct??)

Pdot=A*P+P'*A+Q

where Q=[Q_pos 0 0]
[0 Q_vel 0]
[0 0 Q_accel]

C matrix is: C=[1 0 0]


As I said I measure the position with a noisy sensor and the acceleration
so:

accel = measured_accel - a_bias
vel = vel + accel*dt
pos= pos+vel*dt+1/2*accel*dt^2

Inn=pos_meas - p

And then I update the elements of the matrix Pdot (Pdot=A*P+P'*A+Q)

Then compute the covariance matrix P=P+Pdot*dt

E=CPC'+R

K=PC'inv(E)

Then update the covariance matrix: P=P-KCP

And finally update the state estimate X=X+K*Inn



The results I get from this filter are quite good, but there is something I
am not doing right. When the object is stopped, I get the correct position,
velocity is zero, BUT the a_bias is not correct as it gives me a residual
acceleration that is not zero.
pos=correct (stopped)
vel=0
accel=(a_meas-a_bias)<>0

I don't understand exactly how this kalman filter is implemented (and I
cannot find any info on this way of implementing the filter, I mean updating
the covariace matrix P=P+Pdot*dt).

Can you help me? Do you see anything that I do wrong?

Thanks for your help