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.