The goal here is to put together a Kalman filter to process the GPS and IMU data and produce nav solutions, but first, just to make things easier I wrote a C++ program that would take the data directly from the Arduino, which includes both GPS and IMU data and format it into a single line of output that includes the GPS position data and synchronous IMU for conveninece in filtering. Ultimately, I intend to include a Kalman filter to process the data in real time and produce a Nav solution, but for the time being, the data is written to a file. For filter design and development, matlab is used to process the data. This is a very convenient way to test different filter configurations and plot results for analysis.

A sample of the data formatted for processing follows:

64162.6000   22.1   42.8    5.0    0.6    9.9 -226.3  29.502312 -94.941690  18.000000  6 1
64162.8000   21.3   43.2    5.3   -0.6   10.1 -225.4  29.502312 -94.941692  18.000000  6 1
64163.0000   22.1   42.2    4.6    0.4    9.1 -226.8  29.502310 -94.941692  18.000000  6 1
64163.2000   21.8   42.5    4.6    0.5    9.5 -224.8  29.502310 -94.941692  18.000000  6 1
64163.4000   21.6   42.3    5.4   -0.7   10.7 -225.7  29.502312 -94.941692  18.000000  6 1
64163.6000   21.6   42.4    4.8   -0.1    8.9 -226.4  29.502310 -94.941690  18.000000  6 1

The first column is time and contains the UTC seconds of day. The next three columns are the raw gyro data, the next three are raw accelerometer data, and these are followed by latitude,longitude, and altitude. Latitude and longitude are in fractional degrees and altitude is in meters. The last two columns are the number of satellites being tracked and a flag indicating that GPS nav fixes are available.

The code for the C++ program is not included here because it is a rather trivial exercise in reformatting data. What you need is some code to read a serial port, which is easily found on the internet, algorithms to parse the NMEA messages, which you can borrow from the Arduino GPS library for ADA_fruit, and a few read and write statements to create a file for processing.

The interesting part for me is the filter design, so let's get to it.

Here are the basic equations for a Kalman filter, written in Matlab script:

01: P = F*P*F' + Q;
02: K = P*H'*inv(H*P*H' + R);
03: P = P - K*H*P;
04: x = x + K*Z';

    Definitions:
    P = Error Covariance Matrix
    F = State transition Matrix
    Q = Process Noise Matrix
    R = Measurement covariance matrix 
    K = Kalman Gain
    H = Measurement Partials
    Z = measurement residuals
    x = error state or correction vector

Is that all there is to a Kalman filter? Yep. All we have to do is fill in those matrices and "turn the crank," as we used to say back at Hogwarts School of Aerospace Engineering.

The initial test case I want to analyze is a static case, so I can use the results to tune the filter to solve for attitude misalignment and accelerometer biases. For the initial filter configuration, let us say there are 12 filter states: position (3), velocity (3), attitude (3), and bias (3). The position and velocity states are in earth fixed coordinats, the attitude states and bias states are in the body frame. Since we are considering a static case, we can put the gyro data aside for the moment.

Using matlab script, here is a possible initial error covariance matrix:

     P = [  zeros(3)    zeros(3)   zeros(3)       zeros(3) ;
            zeros(3)    zeros(3)   zeros(3)       zeros(3) ;
            zeros(3)    zeros(3)   .001*eye(3)    zeros(3) ;
            zeros(3)    zeros(3)   zeros(3)   .0001*eye(3)];

Since this is a static case, and we are starting from a known position, I have set the initial position and velocity uncertainties to zero. The initial attitude uncertainty is 1.8 degrees, and the initial accelerometer bias error is 0.01 meters/second/second.

We need to define the state transition matrix, F (F is for Phi, get it?). Here is our matlab script:

     S = [ 0.0   dv(3)  -dv(2)  ;  % skew symmetric matrix
           -dv(3) 0.0     dv(1)  ;
            dv(2) -dv(1)  0.0   ];
     
     % dv = vector of sensed delta velocities since last update.
     % dt = elapsed time since last update
     % T = direction cosine matrix, body to ecf

     F = [ eye(3)   dt*eye(3)  -0.5*dt*T*S  zeros(3)   ;
           zeros(3) eye(3)     -T*S         dt*T       ;
           zeros(3) zeros(3)   eye(3)      zeros(3)    ;
           zeros(3) zeros(3)   zeros(3)    eye(3)     ];

The S matrix, or "skew symmetric matrix," relates angle errors to velocity errors in body coordinates, and it is proportional to the sensed velocity, or delta-V increments. The T matrix transforms the effect into the navigation frame, which is the earth fixed frame in our case. This matrix must be calculated anew for each step, normally, but in a static case, the F matrix is actually constant for a constant time step.

The H matrix, or partials matrix, maps the measurement errors into the state. The measurement is position, so only the position state partials are non-zero:

     H = [ eye(3) zeros(3) zeros(3) zeros(3)];

The Q matrix, or process noise matrix, models the growth in uncertainty due to random dynamic errors. For this demonstration, the following Q matrix is used:

     Q = [ zeros(3) zeros(3)       zeros(3) zeros(3);
           zeros(3) .00001*eye(3)  zeros(3) zeros(3);
           zeros(3) zeros(3)       zeros(3) zeros(3);
           zeros(3) zeros(3)       zeros(3) zeros(3)];

We are assuming that the only random dynamic error of concern is the accelerometer noise, therefore for the time being, all other elements of the Q matrix are set to zero.

We also need to specify the R matrix, which is the covariance of the measurement errors. This should correspond to the uncertainty in the GPS position data, which is about 10 meters.

     R = 100.0*eye(3);

For our static case, the filter is initialized to the current position and the velocity in the earth fixed frame is set to zero. The filter cycle goes as follows:

     % Propagate the position and velocity
     % the sensed acceleration is computed from the accelerometer data
     % and the grav vector is computed using a J2 gravity model.
     r = r + dt*(v + 0.5*dt*(a_sensed_ecf + grav));
     v = v + dt*(a_sensed_ecf + grav);

     % Compute the residuals
     Z = r_gps_ecf - r;

     % Prop the covariance
     P = F*P*F' + Q;

     % Compute the Kalman Gain
     K = P*H'*inv(H*P*H' + R);

     % Update the covariance (if there is a measurement available)
     P = P - K*H*P;

     % Compute the error state or correction vector
     x = K*Z';

     % Apply the correction vector to the navigation variables.
     r = r + x(1:3);
     v = v + x(4:6);
     roll = roll + x(7);
     pitch = pitch + x(8);
     heading = heading + x(9);
     acc_bias = acc_bias + x(10:12);

I think this example illustrates how easily one can set up for testing a Kalman filter implementation using Matlab. It is easy to relate the theoretical equations to the Matlab script and Matlab handles the tedious business of linear algebra, like matrix operations, including matrix inversion.

There you have an overview of the filter formulation and filter algorithm. In the next installment, I will present some test results and some actual code.

Thanks for visiting my website.