EKF for a basic underwater navigation situation

Hello,
I am new to Kalman filters but know the basics fairly well. I have a
device that will be pulled behind a boat (in the y direction) with several
accelerometers and a few gyros - I implemented the linear discrete Kalman
filter but got a terrible integration offset because obviously I have to
integrate the accelerometers twice and the gyros once with the transition
matrix A, state vector xk, control matrix B and control vector u:

    1 T 0 0 0 0 0 0 0         x
    0 1 0 0 0 0 0 0 0         x_dot
    0 0 1 T 0 0 0 0 0         y
A = 0 0 0 1 0 0 0 0 0    xk = y_dot
    0 0 0 0 1 T 0 0 0         z
    0 0 0 0 0 1 0 0 0         z_dot
    0 0 0 0 0 0 1 0 0         phi_x
    0 0 0 0 0 0 0 1 0         phi_y
    0 0 0 0 0 0 0 0 1         phi_z

    0 0 0 0 0 0 0 0 0          0
    0 T 0 0 0 0 0 0 0          x_dot_dot
    0 0 0 0 0 0 0 0 0          0
    0 0 0 T 0 0 0 0 0          y_dot_dot
B = 0 0 0 0 0 0 0 0 0    uk =  0
    0 0 0 0 0 T 0 0 0          z_dot_dot
    0 0 0 0 0 0 T 0 0          phi_x_dot
    0 0 0 0 0 0 0 T 0          phi_y_dot
    0 0 0 0 0 0 0 0 T          phi_z_dot

The uk vector is also the measurement vector zk, as the accelerations and
changes in angle are the sensor outputs I am reading from.

I cannot understand how to change this to conform with the EXTENDED kalman
filter, as I assume this would eliminate the integration offset. I am not
yet looking to translate from the free body frame to the navigation frame
or anything - shouldn't this be simple?!

I am so confused,
any help would be SO appreciated - 
you Kalman dudes are amazing.
Ellie

Sorry - the matrices got all screwed up:

>1 T 0 0 0 0 0 0 0         
>    0 1 0 0 0 0 0 0 0         
>    0 0 1 T 0 0 0 0 0         
>A = 0 0 0 1 0 0 0 0 0    
>    0 0 0 0 1 T 0 0 0    
>    0 0 0 0 0 1 0 0 0    
>    0 0 0 0 0 0 1 0 0    
>    0 0 0 0 0 0 0 1 0    
>    0 0 0 0 0 0 0 0 1         
>
>    0 0 0 0 0 0 0 0 0         
>    0 T 0 0 0 0 0 0 0         
>    0 0 0 0 0 0 0 0 0         
>    0 0 0 T 0 0 0 0 0         
>B = 0 0 0 0 0 0 0 0 0    
>    0 0 0 0 0 T 0 0 0    
>    0 0 0 0 0 0 T 0 0    
>    0 0 0 0 0 0 0 T 0    
>    0 0 0 0 0 0 0 0 T    
>







Ellie

Check 

http://www.taygeta.com/kalman.html
http://www.cs.unc.edu/~tracker/media/pdf/SIGGRAPH2001_CoursePack_08.pdf

In article <25WdnY1nEPRJesDZnZ2dnUVZ_vWdnZ2d@giganews.com>, "elspeth" 
<v_enusinfurs@yahoo.co.nz> wrote:
>Hello,
>I am new to Kalman filters but know the basics fairly well. I have a
>device that will be pulled behind a boat (in the y direction) with several
>accelerometers and a few gyros - I implemented the linear discrete Kalman
>filter but got a terrible integration offset because obviously I have to
>integrate the accelerometers twice and the gyros once with the transition
>matrix A, state vector xk, control matrix B and control vector u:
>
>    1 T 0 0 0 0 0 0 0         x
>    0 1 0 0 0 0 0 0 0         x_dot
>    0 0 1 T 0 0 0 0 0         y
>A = 0 0 0 1 0 0 0 0 0    xk = y_dot
>    0 0 0 0 1 T 0 0 0         z
>    0 0 0 0 0 1 0 0 0         z_dot
>    0 0 0 0 0 0 1 0 0         phi_x
>    0 0 0 0 0 0 0 1 0         phi_y
>    0 0 0 0 0 0 0 0 1         phi_z
>
>    0 0 0 0 0 0 0 0 0          0
>    0 T 0 0 0 0 0 0 0          x_dot_dot
>    0 0 0 0 0 0 0 0 0          0
>    0 0 0 T 0 0 0 0 0          y_dot_dot
>B = 0 0 0 0 0 0 0 0 0    uk =  0
>    0 0 0 0 0 T 0 0 0          z_dot_dot
>    0 0 0 0 0 0 T 0 0          phi_x_dot
>    0 0 0 0 0 0 0 T 0          phi_y_dot
>    0 0 0 0 0 0 0 0 T          phi_z_dot
>
>The uk vector is also the measurement vector zk, as the accelerations and
>changes in angle are the sensor outputs I am reading from.
>
>I cannot understand how to change this to conform with the EXTENDED kalman
>filter, as I assume this would eliminate the integration offset. I am not
>yet looking to translate from the free body frame to the navigation frame
>or anything - shouldn't this be simple?!
>
>I am so confused,
>any help would be SO appreciated - 
>you Kalman dudes are amazing.
>Ellie
>
>

"elspeth" <v_enusinfurs@yahoo.co.nz> wrote in message
news:25WdnY1nEPRJesDZnZ2dnUVZ_vWdnZ2d@giganews.com...
> Hello,
> I am new to Kalman filters but know the basics fairly well. I have a
> device that will be pulled behind a boat (in the y direction) with several
> accelerometers and a few gyros -

You'd better watch that the EKF doesn't get wet. They don't perform well
when they're wet.

M.P

Ok - the extended kalman filter ended up being much like the basic kalman
filter for this situation. The integration offset is still a problem
unfortunately, do anyone know how to avoid this without introducing a
GPS?
Wicked,
thanks.