diff options
| author | Andrew Chambers <achamber@gmail.com> | 2014-07-02 10:04:07 -0700 |
|---|---|---|
| committer | Andrew Chambers <achamber@gmail.com> | 2014-07-02 10:04:07 -0700 |
| commit | 6c6de9395818717916bbc1077fecd51c4db87936 (patch) | |
| tree | c799dd0290aae8719bb3660867979a48ccc5b8d7 /src/modules/uavcan | |
| parent | 8acbe6d5b6770e92fdcb86ba268492217d3e26bd (diff) | |
| download | px4-firmware-6c6de9395818717916bbc1077fecd51c4db87936.tar.gz px4-firmware-6c6de9395818717916bbc1077fecd51c4db87936.tar.bz2 px4-firmware-6c6de9395818717916bbc1077fecd51c4db87936.zip | |
Fixed heading covariance calculation and build errors.
Diffstat (limited to 'src/modules/uavcan')
| -rw-r--r-- | src/modules/uavcan/gnss_receiver.cpp | 17 | ||||
| -rw-r--r-- | src/modules/uavcan/module.mk | 3 |
2 files changed, 14 insertions, 6 deletions
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp index e92468333..490e35bd1 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/gnss_receiver.cpp @@ -80,17 +80,24 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav _report.s_variance_m_s = msg.velocity_covariance[0] + msg.velocity_covariance[4] + msg.velocity_covariance[8]; _report.p_variance_m = msg.position_covariance[0] + msg.position_covariance[4]; - /* Use Jacobian to transform velocity covariance to heading covariance + /* There is a nonlinear relationship between the velocity vector and the heading. + * Use Jacobian to transform velocity covariance to heading covariance + * + * Nonlinear equation: * heading = atan2(vel_e_m_s, vel_n_m_s) * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative * * To calculate the variance of heading from the variance of velocity, - * var(heading) = J(velocity)*var(velocity)*J(velocity)^T + * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T */ + float vel_n = msg.ned_velocity[0]; + float vel_e = msg.ned_velocity[1]; + float vel_n_sq = vel_n * vel_n; + float vel_e_sq = vel_e * vel_e; _report.c_variance_rad = - msg.ned_velocity[1] * msg.ned_velocity[1] * msg.velocity_covariance[0] + - -2*msg.ned_velocity[1] * msg.ned_velocity[0] * msg.velocity_covariance[1] + - msg.ned_velocity[0] * msg.ned_velocity[0] * msg.velocity_covariance[4]; + (vel_e_sq * msg.velocity_covariance[0] + + -2 * vel_n * vel_e * msg.velocity_covariance[1] + // Covariance matrix is symmetric + vel_n_sq* msg.velocity_covariance[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); _report.fix_type = msg.status; diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 5ac7019e3..2c75944d4 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -42,7 +42,8 @@ MAXOPTIMIZATION = -Os SRCS += uavcan_main.cpp \ uavcan_clock.cpp \ - esc_controller.cpp + esc_controller.cpp \ + gnss_receiver.cpp # # libuavcan |