fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 11:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support vision velocity expressed in body frame too (#708)

Browse Source 
* Support vision velocity expressed in body frame

* Use switch statement for vision velocity frame

* Robustify vision velocity frame test

* Increase lower bound on vision velocity noise to 0.05 m/s
This commit is contained in:
kritz
2020-05-12 16:03:35 +02:00
committed by GitHub
parent c3de452e8e
commit 98801ad17b
8 changed files with 170 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/vio.cpp
+19 -2
View File
@@ -26,7 +26,12 @@ void Vio::setData(const extVisionSample& vio_data)
void Vio::setVelocityVariance(const Vector3f& velVar)
{
_vio_data.velVar = velVar;
setVelocityCovariance(matrix::diag(velVar));
}
void Vio::setVelocityCovariance(const Matrix3f& velCov)
{
_vio_data.velCov = velCov;
}
void Vio::setPositionVariance(const Vector3f& posVar)
@@ -54,6 +59,17 @@ void Vio::setOrientation(const Quatf& quat)
_vio_data.quat = quat;
}
void Vio::setVelocityFrameToBody()
{
_vio_data.vel_frame = BODY_FRAME_FRD;
}
void Vio::setVelocityFrameToLocal()
{
_vio_data.vel_frame = LOCAL_FRAME_FRD;
}
extVisionSample Vio::dataAtRest()
{
extVisionSample vio_data;
@@ -61,8 +77,9 @@ extVisionSample Vio::dataAtRest()
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velCov = matrix::eye<float ,3>() * 0.1f;
vio_data.angVar = 0.05f;
vio_data.vel_frame = LOCAL_FRAME_FRD;
return vio_data;
}