Attitude from MOCAP

Follow up from #265
@mdouglas90 wrote:

In the mean time, I've written the code that sends the ahrs quaternion to the drone from the motion capture system. It currently works by writing over the ahrs quaternion every time the message is received. This also allows us to completely bypass the strange magnetic field that is in the dronedome. The code still calls the ekf update function so there is still some decay but it will jump back when it receives the next mocap message. If this decay is still not wanted and the update rate is not important, you can comment out the body of Ahrs_ekf::update() and the drone will just update the ahrs quaternion at 10 Hz.

The branch locations for this code are:

MAVRIC branch: dronedome_ahrs
natnetclient branch; dronedome_ahrs

Does it fly?
I does not sound like a good idea to comment Ahrs_ekf::update() because the angular speed would not be updated.

The proper way would be to apply a linear kalman update with the mocap quaternion:

z = [q0]
    [q1]
    [q2]
    [q3]

H = [0, 0, 0, 0, 1, 0, 0, 0]
    [0, 0, 0, 0, 0, 1, 0, 0]
    [0, 0, 0, 0, 0, 0, 1, 0]
    [0, 0, 0, 0, 0, 0, 0, 1]

S = [s, 0, 0, 0]
    [0, s, 0, 0]
    [0, 0, s, 0]
    [0, 0, 0, s]

with s << s_accelerometer and s << s_magnetometer

It has not yet been tested in flight.

This method is more of temporary solution.

By applying the kalman update on the mocap quaternion, we might need to reorganize the ahrs_ekf class if we do not want to have two different versions of those files. The reason being that Ahrs_ekf::update() calls all the updates and then sets the ahrs structure. One possible solution would be to have a virtual function called Ahrs_ekf::update_from_sensors() which then updates the ekf based on all of the sensors with the default being just the magnetometer and accelerometer. We could then instantiate a child class for the mocap system that would include the imu and the mocap quaternion. There are probably other options as well.

Yes, another option that does require a child class specific to mocap is the following:

Have ahrs_ekf derive from Kalman
from the callback to mocap messages, call ahrs_ekf::update(z, H, R) see here
Have ahrs_ekf derive also from ahrs, so that there is no need to copy data from the state vector to ahrs structure

@jlecoeur

Do you know what the point of this line is. It declares a boolean and then uses it without assigning it.

The boolean indicates if the matrix could be inverted
Le 16 juin 2016 5:41 PM, "mdouglas90" notifications@github.com a écrit :

@jlecoeur https://github.com/jlecoeur

Do you know what the point of this line

MAVRIC_Library/util/kalman.hpp

Line 144 in 567f12d

bool inversible;

is. It declares a boolean and then uses it without assigning it.

—
You are receiving this because you were mentioned.
Reply to this email directly, view it on GitHub
#266 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/ABUMh54XxVsqMjTSPs1660B3awBavtcsks5qMW6MgaJpZM4I0W4s
.

The question was more about the fact that it is never assigned. It just uses whatever the compiler assigns it.

Check the declaration of Mat::inv()

    /**
     * \brief  Get inversed matrix
     *
     * \param   success     Indicates if the matrix could be inverted
     *
     * \return  result
     */
Mat inv(bool& success) const;

the initial value of the boolean does not matter.

How do we want to go about including the Ahrs_ekf_mocap into LEQuad/LEQuad_dronedome. Currently, the Ahrs_ekf module is created in the constructor of LEQuad, but if we want to allow a child Ahrs_ekf_mocap to be created, it should be in LEQuad_dronedome. I could make a second constructor for LEQuad that just sets all the variables and then all of modules would be created in the subclass, but that doesn't seem ideal.

We didn't have this issue with the mocap position because it was a child of Gps which is a variable that is passed into LEQuad.

What about a separate module ahrs_ekf_telemetry that initialises the mavlink callback, and contains the required matrices ?