Adding two position sensors within the ssf_update

[alek-b]

Hi,
for the localization using the AscTec Pelican, we would use two different position sensors (one @ 10 Hz, the other one @ 50 Hz).
Both provide absolute position of the UAV with respect to the world reference system.

We already wrote the two callbacks and the data are received correctly within the ssf_update.

We estimated the standard deviation for both sensors and used fixed covariance within the ssf_update.

So we modified the code as follows:

...
#define N_MEAS 12 // measurement size (x,y,z from first sensor; x,y,z from the second sensor, the others concerns the artificial measurements that we keep fix)

...
...

callback_faster(...)
{
    // measurements
    z_p_ = Eigen::Matrix<double,3,1>(msg->position.x, msg->position.y,msg->position.z);
    z_p_2 = Eigen::Matrix<double,3,1>(msg2->position.x, msg2->position.y,msg2->position.z);

    ...

    // H matrix (we use the same expression since both sensors provide absolute position of the UAV
    H_old.block<3,3>(0,0) = C_wv.transpose()*state_old.L_; // p first sensor
    H_old.block<3,3>(0,0) = C_wv.transpose()*state_old.L_; // p
    H_old.block<3,3>(0,6) = -C_wv.transpose()*C_q.transpose()*pci_sk*state_old.L_; // q
    H_old.block<3,1>(0,15) = C_wv.transpose()*C_q.transpose()*state_old.p_ci_ + C_wv.transpose()*state_old.p_; // L
    H_old.block<3,3>(0,16) = -C_wv.transpose()*skewold; // q_wv

    H_old.block<3,3>(3,0) = C_wv.transpose()*state_old.L_; // p second sensor

    ...

    // residual
    r_old.block<3,1>(0,0) = z_p_ - C_wv.transpose()*(state_old.p_ + C_q.transpose()*state_old.p_ci_)*state_old.L_;
    r_old.block<3,1>(3,0) = z_p_2 - C_wv.transpose()*(state_old.p_ + C_q.transpose()*state_old.p_ci_)*state_old.L_;

    ...
}

In this way the output of the filter diverges even if the two set of position measurements are very close each other.
Are we missing something?
Maybe we have to add also:

    H_old.block<3,3>(3,0) = C_wv.transpose()*state_old.L_; // p
    H_old.block<3,3>(3,6) = -C_wv.transpose()*C_q.transpose()*pci_sk*state_old.L_; // q
    H_old.block<3,1>(3,15) = C_wv.transpose()*C_q.transpose()*state_old.p_ci_ + C_wv.transpose()*state_old.p_; // L
    H_old.block<3,3>(3,16) = -C_wv.transpose()*skewold; // q_wv

Do you have some suggestion?

Thanks

[stephanweiss]

Hi,

Sorry for the late reply.

What is the reason to use the slower sensor (i.e. what is the difference between the sensors)?

If you use 2 position sensors, in practice, p_ic (the distance between the sensor and the IMU) is likely to be different for each sensor. This could lead to issues.

Does the filter work with the faster sensor only / with the slower sensor only?

Best
Stephan

---

From: AB [notifications@github.com]
Sent: Thursday, August 22, 2013 4:46 AM
To: ethz-asl/ethzasl_sensor_fusion
Subject: [ethzasl_sensor_fusion] Adding two position sensors within the ssf_update (#14)

Hi,
for the localization using the AscTec Pelican, we would use two different position sensors (one @ 10 Hz, the other one @ 50 Hz).
Both provide absolute position of the UAV with respect to the world reference system.

We already wrote the two callbacks and the data are received correctly within the ssf_update.

So we modified the code as follows:

#define N_MEAS 6 // measurement size (x,y,z from first sensor; x,y,z from the second sensor)

...
...

callback_faster(...)
{
// measurements
z_p_ = Eigen::Matrix(msg->position.x, msg->position.y,msg->position.z);
z_p_2 = Eigen::Matrix(msg2->position.x, msg2->position.y,msg2->position.z);

...

// H matrix
H_old.block<3,3>(0,0) = C_wv.transpose()state_old.L; // p first sensor
H_old.block<3,3>(3,0) = C_wv.transpose()state_old.L; // p second sensor

...

// residual
r_old.block<3,1>(0,0) = z_p_ - C_wv.transpose()(state_old.p + C_q.transpose()state_old.p_ci)state_old.L;
r_old.block<3,1>(3,0) = z_p_2 - C_wv.transpose()(state_old.p + C_q.transpose()state_old.p_ci)state_old.L;

...

}

In this way the output of the filter diverges even if the two set of position measurements are very close each other.
Are we missing something?
Do you have some suggestion?

Thanks

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/14.

[alek-b]

Hi Stephan,
the objective of our work is to get the UAV landing autonomously on a platform (fixed at a known position).
Right now we have the UWB System that covers all the area but its accuracy is around 20cm.
In addition we have a vision system that is capable to estimate the distance from the center of the marker but only when the UAV is close to it (the vision system works only when the marker is within the point of view of the camera).
The accuracy of the vision system is around 2 cm.

The UWB works @ 10 Hz while the vision works @ 50 Hz.

The idea should be to use both positions sensor.

Maybe could be better to switch between the two position sensors?
Could this generate a stability issue?

The filter works fine with the slower sensor. We are testing the faster one.

Bye

Alessandro

[stephanweiss]

Alessandro,

Thanks for the info.

If both sensors work separately then it should work when used together. Please test first if the system works when using only one sensor. Also, align both global measurements to the same origin and attitude (incl. yaw).

Best
Stephan

---

From: AB [notifications@github.com]
Sent: Monday, August 26, 2013 2:22 PM
To: ethz-asl/ethzasl_sensor_fusion
Cc: stephanweiss
Subject: Re: [ethzasl_sensor_fusion] Adding two position sensors within the ssf_update (#14)

Hi Stephan,
the objective of our work is to get the UAV landing autonomously on a platform (fixed at a known position).
Right now we have the UWB System that covers all the area but its accuracy is around 20cm.
In addition we have a vision system that is capable to estimate the distance from the center of the marker but only when the UAV is close to it (the vision system works only when the marker is within the point of view of the camera).
The UWB works @ 10 Hz while the vision works @ 50 Hz.
The idea should be to use both positions sensor.

Maybe could be better to switch between the two position sensors?

Bye

Alessandro

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/14#issuecomment-23295925.

[alek-b]

Stephan,
just for curiosity:
what do you think about switching between the two position sensors?
Do you agree with out implementation (see above)?

Thanks

Alessandro

[stephanweiss]

You can do the switching. If it runs with both sensors you would circumvent issues with:

• switching strategy
• failures in one sensor

So I would encourage you to make it work with both sensors simultaneously

Best
Stephan

---

From: AB [notifications@github.com]
Sent: Monday, August 26, 2013 2:50 PM
To: ethz-asl/ethzasl_sensor_fusion
Cc: stephanweiss
Subject: Re: [ethzasl_sensor_fusion] Adding two position sensors within the ssf_update (#14)

Stephan,
just for curiosity:
what do you think about switching between the two position sensors?

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/14#issuecomment-23297962.