Kalman Filter implementation for a drone. Help confirming my sensor inputs are correct.

edit

Hi, I'm learning more about the kalman filter and I thought I'd write a simple implementation for position tracking based on the details at this link and this link

However, The filter estimates and the real sensor readings are far off. I'm using a Matrice 100 drone and subscribing to the following messages to populate my state estimate:

/dji_sdk/imu ( publishes imu data)
/dji_sdk/velocity ( publishes velocity)
/dji_sdk/local_position ( publishes local position in Cartesian coordinates)

My state estimate is a 9 x 1 vector tracking position, velocity and acceleration

x << pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z;

I populate these variables using the following which I get from the callbacks above:

pos_x = local_position.point.x;
pos_y = local_position.point.y;
pos_z = local_position.point.z;
vel_x = current_velocity.vector.x;
vel_y = current_velocity.vector.y;
vel_z = current_velocity.vector.z;
acc_x = current_acceleration.linear_acceleration.x;
acc_y = current_acceleration.linear_acceleration.y;
acc_z = current_acceleration.linear_acceleration.z;

I defined the rest of the filter as

 MatrixXd A(9,9);
    A <<    1, 0, 0, dt, 0, 0, dt_squared, 0 , 0,
            0, 1, 0, 0,  dt, 0, 0, dt_squared, 0,
            0, 0, 1, 0, 0, dt, 0, 0, dt_squared,
            0, 0, 0, 1, 0, 0, dt, 0, 0,
            0, 0, 0, 0, 1, 0, 0 , dt, 0,
            0, 0, 0, 0, 0, 1, 0, 0, dt,
            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;

    // State Covariance Matrix
MatrixXd P(9,9);
P<< 100, 0, 0, 0, 0, 0, 0, 0, 0,
        0, 100, 0, 0, 0, 0, 0, 0, 0,
        0, 0, 100, 0, 0, 0, 0, 0, 0,
        0, 0, 0, 100, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 100, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 100, 0, 0, 0,
        0, 0, 0, 0, 0, 0, 100, 0, 0,
        0, 0, 0, 0, 0, 0, 0, 100, 0,
        0, 0, 0, 0, 0, 0, 0, 0, 100;


MatrixXd H = MatrixXd::Identity(3,9);

  // Measurement noise Matrix. 
 MatrixXd R(3,3);
R << 0.1, 0, 0,
     0, 0.1, 0,
     0, 0, 0.1;

and I also defined an appropriate Q Matrix based on the CA model as defined in the second link.

I initialise the filter:

One of the things I'm not sure of is the values of my measurements vector z . I've defined it as

z << pos_x , pos_y , pos_z;

which is also technically the first three elements of my state vector. Is this correct?

I then run the filter at 100Hz which is the rate at which the drone publishes the messages but my results are shown at this link:)

Is anyone able to help share some pointers on this? Also, here's a link to my current source code if anyone is willing to take a look and see what I'm doing wrong.