ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Tue, 14 Aug 2018 08:15:42 -0500Kalman Filter implementation for a drone. Help confirming my sensor inputs are correct.https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/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](http://campar.in.tum.de/Chair/KalmanFilter) and this [link](http://nbviewer.jupyter.org/github/balzer82/Kalman/blob/master/Kalman-Filter-CA-Ball.ipynb?create=1)
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:](https://imgur.com/a/hL3RScW))
Is anyone able to help share some pointers on this? Also, here's a [link](https://github.com/InterestingWalrus/kf_tester/blob/master/src/kf_node.cpp) to my current source code if anyone is willing to take a look and see what I'm doing wrong.Mon, 13 Aug 2018 16:48:31 -0500https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/Comment by marinePhD for <p>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 <a href="http://campar.in.tum.de/Chair/KalmanFilter">link</a> and this <a href="http://nbviewer.jupyter.org/github/balzer82/Kalman/blob/master/Kalman-Filter-CA-Ball.ipynb?create=1">link</a></p>
<p>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:</p>
<p>/dji_sdk/imu ( publishes imu data) <br>
/dji_sdk/velocity ( publishes velocity) <br>
/dji_sdk/local_position ( publishes local position in Cartesian coordinates)</p>
<p>My state estimate is a 9 x 1 vector tracking position, velocity and acceleration</p>
<pre><code>x << pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z;
</code></pre>
<p>I populate these variables using the following which I get from the callbacks above:</p>
<pre><code>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;
</code></pre>
<p>I defined the rest of the filter as </p>
<pre><code> 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;
</code></pre>
<p>and I also defined an appropriate Q Matrix based on the CA model as defined in the second link. </p>
<p>I initialise the filter: </p>
<p>One of the things I'm not sure of is the values of my measurements vector <code>z</code> . I've defined it as </p>
<pre><code>z << pos_x , pos_y , pos_z;
</code></pre>
<p>which is also technically the first three elements of my state vector. <strong>Is this correct?</strong></p>
<p>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 <a href="https://imgur.com/a/hL3RScW">link:</a>)</p>
<p>Is anyone able to help share some pointers on this? Also, here's a <a href="https://github.com/InterestingWalrus/kf_tester/blob/master/src/kf_node.cpp">link</a> to my current source code if anyone is willing to take a look and see what I'm doing wrong.</p>
https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300564#post-id-300564@PeteBlackerThe3rd Thanks for your comment, I set an identity matrix of 0.05 and played with the standard deviation for my Q for position and the filter performs close to what I expected.Tue, 14 Aug 2018 08:15:42 -0500https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300564#post-id-300564Comment by marinePhD for <p>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 <a href="http://campar.in.tum.de/Chair/KalmanFilter">link</a> and this <a href="http://nbviewer.jupyter.org/github/balzer82/Kalman/blob/master/Kalman-Filter-CA-Ball.ipynb?create=1">link</a></p>
<p>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:</p>
<p>/dji_sdk/imu ( publishes imu data) <br>
/dji_sdk/velocity ( publishes velocity) <br>
/dji_sdk/local_position ( publishes local position in Cartesian coordinates)</p>
<p>My state estimate is a 9 x 1 vector tracking position, velocity and acceleration</p>
<pre><code>x << pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z;
</code></pre>
<p>I populate these variables using the following which I get from the callbacks above:</p>
<pre><code>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;
</code></pre>
<p>I defined the rest of the filter as </p>
<pre><code> 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;
</code></pre>
<p>and I also defined an appropriate Q Matrix based on the CA model as defined in the second link. </p>
<p>I initialise the filter: </p>
<p>One of the things I'm not sure of is the values of my measurements vector <code>z</code> . I've defined it as </p>
<pre><code>z << pos_x , pos_y , pos_z;
</code></pre>
<p>which is also technically the first three elements of my state vector. <strong>Is this correct?</strong></p>
<p>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 <a href="https://imgur.com/a/hL3RScW">link:</a>)</p>
<p>Is anyone able to help share some pointers on this? Also, here's a <a href="https://github.com/InterestingWalrus/kf_tester/blob/master/src/kf_node.cpp">link</a> to my current source code if anyone is willing to take a look and see what I'm doing wrong.</p>
https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300546#post-id-300546@PeteBlackerThe3rd, Currently not. Part of current work is to determine if the discrete kalman filter was sufficient to track position in 3D if your sensor gave you 3D positions and compare against a package like robot_localization for example. Take this [paper](https://arxiv.org/pdf/1611.07329.pdf)Tue, 14 Aug 2018 06:41:40 -0500https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300546#post-id-300546Comment by marinePhD for <p>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 <a href="http://campar.in.tum.de/Chair/KalmanFilter">link</a> and this <a href="http://nbviewer.jupyter.org/github/balzer82/Kalman/blob/master/Kalman-Filter-CA-Ball.ipynb?create=1">link</a></p>
<p>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:</p>
<p>/dji_sdk/imu ( publishes imu data) <br>
/dji_sdk/velocity ( publishes velocity) <br>
/dji_sdk/local_position ( publishes local position in Cartesian coordinates)</p>
<p>My state estimate is a 9 x 1 vector tracking position, velocity and acceleration</p>
<pre><code>x << pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z;
</code></pre>
<p>I populate these variables using the following which I get from the callbacks above:</p>
<pre><code>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;
</code></pre>
<p>I defined the rest of the filter as </p>
<pre><code> 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;
</code></pre>
<p>and I also defined an appropriate Q Matrix based on the CA model as defined in the second link. </p>
<p>I initialise the filter: </p>
<p>One of the things I'm not sure of is the values of my measurements vector <code>z</code> . I've defined it as </p>
<pre><code>z << pos_x , pos_y , pos_z;
</code></pre>
<p>which is also technically the first three elements of my state vector. <strong>Is this correct?</strong></p>
<p>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 <a href="https://imgur.com/a/hL3RScW">link:</a>)</p>
<p>Is anyone able to help share some pointers on this? Also, here's a <a href="https://github.com/InterestingWalrus/kf_tester/blob/master/src/kf_node.cpp">link</a> to my current source code if anyone is willing to take a look and see what I'm doing wrong.</p>
https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300548#post-id-300548@PeteBlackerThe3rd as an example, There work is similar to the stuff I'm trying to validate and they don't seem to be tracking the orientation of the drone.Tue, 14 Aug 2018 06:42:34 -0500https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300548#post-id-300548Comment by PeteBlackerThe3rd for <p>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 <a href="http://campar.in.tum.de/Chair/KalmanFilter">link</a> and this <a href="http://nbviewer.jupyter.org/github/balzer82/Kalman/blob/master/Kalman-Filter-CA-Ball.ipynb?create=1">link</a></p>
<p>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:</p>
<p>/dji_sdk/imu ( publishes imu data) <br>
/dji_sdk/velocity ( publishes velocity) <br>
/dji_sdk/local_position ( publishes local position in Cartesian coordinates)</p>
<p>My state estimate is a 9 x 1 vector tracking position, velocity and acceleration</p>
<pre><code>x << pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z;
</code></pre>
<p>I populate these variables using the following which I get from the callbacks above:</p>
<pre><code>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;
</code></pre>
<p>I defined the rest of the filter as </p>
<pre><code> 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;
</code></pre>
<p>and I also defined an appropriate Q Matrix based on the CA model as defined in the second link. </p>
<p>I initialise the filter: </p>
<p>One of the things I'm not sure of is the values of my measurements vector <code>z</code> . I've defined it as </p>
<pre><code>z << pos_x , pos_y , pos_z;
</code></pre>
<p>which is also technically the first three elements of my state vector. <strong>Is this correct?</strong></p>
<p>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 <a href="https://imgur.com/a/hL3RScW">link:</a>)</p>
<p>Is anyone able to help share some pointers on this? Also, here's a <a href="https://github.com/InterestingWalrus/kf_tester/blob/master/src/kf_node.cpp">link</a> to my current source code if anyone is willing to take a look and see what I'm doing wrong.</p>
https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300542#post-id-300542Are you not tracking the orientation of the drone? Without that information your 3D values are meaningless.Tue, 14 Aug 2018 06:33:58 -0500https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?comment=300542#post-id-300542Answer by PeteBlackerThe3rd for <p>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 <a href="http://campar.in.tum.de/Chair/KalmanFilter">link</a> and this <a href="http://nbviewer.jupyter.org/github/balzer82/Kalman/blob/master/Kalman-Filter-CA-Ball.ipynb?create=1">link</a></p>
<p>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:</p>
<p>/dji_sdk/imu ( publishes imu data) <br>
/dji_sdk/velocity ( publishes velocity) <br>
/dji_sdk/local_position ( publishes local position in Cartesian coordinates)</p>
<p>My state estimate is a 9 x 1 vector tracking position, velocity and acceleration</p>
<pre><code>x << pos_x, pos_y, pos_z, vel_x, vel_y, vel_z, acc_x, acc_y, acc_z;
</code></pre>
<p>I populate these variables using the following which I get from the callbacks above:</p>
<pre><code>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;
</code></pre>
<p>I defined the rest of the filter as </p>
<pre><code> 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;
</code></pre>
<p>and I also defined an appropriate Q Matrix based on the CA model as defined in the second link. </p>
<p>I initialise the filter: </p>
<p>One of the things I'm not sure of is the values of my measurements vector <code>z</code> . I've defined it as </p>
<pre><code>z << pos_x , pos_y , pos_z;
</code></pre>
<p>which is also technically the first three elements of my state vector. <strong>Is this correct?</strong></p>
<p>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 <a href="https://imgur.com/a/hL3RScW">link:</a>)</p>
<p>Is anyone able to help share some pointers on this? Also, here's a <a href="https://github.com/InterestingWalrus/kf_tester/blob/master/src/kf_node.cpp">link</a> to my current source code if anyone is willing to take a look and see what I'm doing wrong.</p>
https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?answer=300555#post-id-300555In order for their experiment to work then the three sets of XYZ values need to be in the same frame. In your case here the IMU may be publishing linear acceleration data in a different coordinate system to the local_position data. This will make life impossible for the Kalman filter.Tue, 14 Aug 2018 07:55:25 -0500https://answers.ros.org/question/300491/kalman-filter-implementation-for-a-drone-help-confirming-my-sensor-inputs-are-correct/?answer=300555#post-id-300555