# Arduino Forum

## Topics => Science and Measurement => Topic started by: catalin_dragosh on Mar 14, 2011, 01:50 pm

Title: Inertial Navigation Integration FIlter
Post by: catalin_dragosh on Mar 14, 2011, 01:50 pm
Hello,
I am trying to create a filter that can integrate inertial measurements with GPS measurements. My inertial sensors are a yaw axis gyro, an odometer, and possibly a 3axis accelerometer.
Could you please help me with some guidance? I have read papers, articles and books on the subject but I am still a little confused.
I consider the acceleration constant, so I have a state vector x=[E N v_E v_N a_E a_N], E and N are the positions in NED coordinate system, v_E and v_N are the speed on each axis, and a is the acceleration on each axis
The measurement vector z is [E_GPS N_GPS E_DR N_DR a_E a_N] where E_GPS and N_GPS are the coordinates obtained from the GPS receiver and E_DR and N_DR are the coordinates obtained from the inertial sensors through dead reckoning, and a is the acceleration obtained from the accelerometer.
F, the transformation matrix is: [1 0 dt 0 (dt^2)/2 0; 0 1 0 dt 0 (dt^2)/2; 0 0 1 0 dt 0; 0 0 0 1 0 dt; 0 0 0 0 0 1];
H, the measurement matrix is: [1 0 0 0 0 0; 0 1 0 0 0 0 ; 1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1],
Does it make any sense so far? Should I chose a different state vector?
My problem is that I don't know, for this system how can I determine the process noise covariance and measurement noise covariance.
Can you give me some indications in that direction?
Thank you very much.
Title: Re: Inertial Navigation Integration FIlter
Post by: RuggedCircuits on Mar 14, 2011, 04:13 pm
It sounds like you are building a Kalman Filter, yes? You may want to lower the number of states as they are interdependent. Really, the "state" of your object is just a position (x,y,z) and orientation (i.e., rotation about the axes xr,yr). The other states can be derived as derivatives.

As for determining the noise covariances, there are several options:

1) Make it up. This seems to be what most people do.
2) Come up with an analytical model based upon a deep understanding of your system and its sensors.
3) Measure it. This is probably the most practical.

Suppose you keep your object perfectly still. Measure the accelerometer outputs and compute the variance on each signal output due to noise. Repeat for other sensors. The process noise covariance is harder to get at.

Either way, this is pretty advanced work and unless you are in graduate school you are unlikely to "do this right" so you may just want to try approach #1 and make it up. If you set your variances too high then the sensor readings will tend to be over-ignored and most of the computation of state updates will be based upon your physical model of inertial updating. If you set your variances too low then the sensor readings will be trusted over the inertial model but if the sensors are noisy you will then get poor updating state estimates.

Good luck.

--
The Rugged Motor Driver (http://ruggedcircuits.com/html/rugged_motor_driver.html): two H-bridges, more power than an L298, fully protected
Title: Re: Inertial Navigation Integration FIlter
Post by: catalin_dragosh on Mar 16, 2011, 04:32 pm
Hello,
Thank you for the reply.
Should I introduce also the odometer and gyro bias as elements in the state vector? Will I see big differences in the results if I don't introduce the biases? From my understanding..if I dont put them in the state vector, they will pass as measurement error, and therefore I cannot correct my odometer and gyro measurements...is it correct? did i get it well?
Thank you
Title: Re: Inertial Navigation Integration FIlter
Post by: RuggedCircuits on Mar 16, 2011, 04:48 pm
It depends, if your bias is constant and can be characterized you can just subtract it out of each measurement before you even inject it into the Kalman filter.

Otherwise you could try to make the bias a state and estimate it along with everything else, though like everything else you will need an inertial model for the bias as well as estimates of process and noise covariance. Not easy.

--
The Gadget Shield (http://ruggedcircuits.com/html/gadget_shield.html): accelerometer, RGB LED, IR transmit/receive, speaker, microphone, light sensor, potentiometer, pushbuttons