robot_localization中EKF源码介绍

/** Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc.* Copyright (c) 2017, Locus Robotics, Inc.* All rights reserved.** Redistribution and use in source and binary forms, with or without* modification, are permitted provided that the following conditions* are met:** 1. Redistributions of source code must retain the above copyright* notice, this list of conditions and the following disclaimer.* 2. Redistributions in binary form must reproduce the above* copyright notice, this list of conditions and the following* disclaimer in the documentation and/or other materials provided* with the distribution.* 3. Neither the name of the copyright holder nor the names of its* contributors may be used to endorse or promote products derived* from this software without specific prior written permission.** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE* POSSIBILITY OF SUCH DAMAGE.*/
//其参考的导航坐标系为东北天坐标系
#include <robot_localization/ekf.hpp>
#include <robot_localization/filter_common.hpp>
#include <angles/angles.h>
#include <Eigen/Dense>
#include <rclcpp/duration.hpp>
#include <vector>namespace robot_localization
{
Ekf::Ekf()
: FilterBase()
{}Ekf::~Ekf() {}
//扩展卡尔曼滤波的量测更新
void Ekf::correct(const Measurement & measurement)
{FB_DEBUG("---------------------- Ekf::correct ----------------------\n" <<"State is:\n" <<state_ <<"\n""Topic is:\n" <<measurement.topic_name_ <<"\n""Measurement is:\n" <<measurement.measurement_ <<"\n""Measurement topic name is:\n" <<measurement.topic_name_ <<"\n\n""Measurement covariance is:\n" <<measurement.covariance_ << "\n");// We don't want to update everything, so we need to build matrices that only// update the measured parts of our state vector. Throughout prediction and// correction, we attempt to maximize efficiency in Eigen.// First, determine how many state vector values we're updatingstd::vector<size_t> update_indices;for (size_t i = 0; i < measurement.update_vector_.size(); ++i) {if (measurement.update_vector_[i]) {// Handle nan and inf values in measurementsif (std::isnan(measurement.measurement_(i))) {FB_DEBUG("Value at index " << i <<" was nan. Excluding from update.\n");} else if (std::isinf(measurement.measurement_(i))) {FB_DEBUG("Value at index " << i <<" was inf. Excluding from update.\n");} else {update_indices.push_back(i);}}}FB_DEBUG("Update indices are:\n" << update_indices << "\n");size_t update_size = update_indices.size();//设置状态量的向量和相关矩阵 Eigen::VectorXd是表示的是库Eigen的向量，而MatrixXd为其的矩阵// Now set up the relevant matricesEigen::VectorXd state_subset(update_size);       // x (in most literature)Eigen::VectorXd measurement_subset(update_size);  // zEigen::MatrixXd measurement_covariance_subset(update_size, update_size);  // REigen::MatrixXd state_to_measurement_subset(update_size, state_.rows());  // H 测量矩阵Eigen::MatrixXd kalman_gain_subset(state_.rows(), update_size);          // K Eigen::VectorXd innovation_subset(update_size);  // z - Hx 观测误差//将所有矩阵、向量初始化为0state_subset.setZero();measurement_subset.setZero();measurement_covariance_subset.setZero();state_to_measurement_subset.setZero();kalman_gain_subset.setZero();innovation_subset.setZero();// Now build the sub-matrices from the full-sized matricesfor (size_t i = 0; i < update_size; ++i) {measurement_subset(i) = measurement.measurement_(update_indices[i]);state_subset(i) = state_(update_indices[i]);for (size_t j = 0; j < update_size; ++j) {measurement_covariance_subset(i, j) =measurement.covariance_(update_indices[i], update_indices[j]);}// Handle negative (read: bad) covariances in the measurement. Rather// than exclude the measurement or make up a covariance, just take// the absolute value.//设置量测协方差矩阵//检查量测协方差矩阵是否为正，若不为正，则通过绝对值函数fabs将其改为正，防止矩阵奇异if (measurement_covariance_subset(i, i) < 0) {FB_DEBUG("WARNING: Negative covariance for index " <<i << " of measurement (value is" <<measurement_covariance_subset(i, i) <<"). Using absolute value...\n");measurement_covariance_subset(i, i) =::fabs(measurement_covariance_subset(i, i));}// If the measurement variance for a given variable is very// near 0 (as in e-50 or so) and the variance for that// variable in the covariance matrix is also near zero, then// the Kalman gain computation will blow up. Really, no// measurement can be completely without error, so add a small// amount in that case.if (measurement_covariance_subset(i, i) < 1e-9) {FB_DEBUG("WARNING: measurement had very small error covariance for index " <<update_indices[i] <<". Adding some noise to maintain filter stability.\n");measurement_covariance_subset(i, i) = 1e-9;}}// The state-to-measurement function, h, will now be a measurement_size x// full_state_size matrix, with ones in the (i, i) locations of the values to// be updated//设置量测矩阵H的值for (size_t i = 0; i < update_size; ++i) {state_to_measurement_subset(i, update_indices[i]) = 1;}FB_DEBUG("Current state subset is:\n" <<state_subset << "\nMeasurement subset is:\n" <<measurement_subset << "\nMeasurement covariance subset is:\n" <<measurement_covariance_subset <<"\nState-to-measurement subset is:\n" <<state_to_measurement_subset << "\n");// (1) Compute the Kalman gain: K = (PH') / (HPH' + R) 计算卡尔曼增益//transpose()转置函数 inverse()求逆函数 noalias()为防止左右赋值时出现混淆Eigen::MatrixXd pht =estimate_error_covariance_ * state_to_measurement_subset.transpose();Eigen::MatrixXd hphr_inverse =(state_to_measurement_subset * pht + measurement_covariance_subset).inverse();kalman_gain_subset.noalias() = pht * hphr_inverse;innovation_subset = (measurement_subset - state_subset);// Wrap angles in the innovationfor (size_t i = 0; i < update_size; ++i) {if (update_indices[i] == StateMemberRoll ||update_indices[i] == StateMemberPitch ||update_indices[i] == StateMemberYaw){innovation_subset(i) = ::angles::normalize_angle(innovation_subset(i));//调用ROS中angles的normalize_angle，将角度装为弧度范围为【-PI,PI】}}// (2) Check Mahalanobis distance between mapped measurement and state. 检查状态值与测量值之间的马氏距离：Mahalanobis距离是数据所在的空间的协方差的度量if (checkMahalanobisThreshold(innovation_subset, hphr_inverse,measurement.mahalanobis_thresh_)){// (3) Apply the gain to the difference between the state and measurement: x 利用当前观测与观测结果获得最终融合结果// x= x + K(z - Hx)state_.noalias() += kalman_gain_subset * innovation_subset;// (4) Update the estimate error covariance using the Joseph form: P=(I -KH)P(I - KH)' + KRK'//获得最终噪声的协方差矩阵Eigen::MatrixXd gain_residual = identity_;//单位矩阵gain_residual.noalias() -= kalman_gain_subset * state_to_measurement_subset;estimate_error_covariance_ =gain_residual * estimate_error_covariance_ * gain_residual.transpose();//(I -KH)P(I - KH)'estimate_error_covariance_.noalias() += kalman_gain_subset *measurement_covariance_subset *kalman_gain_subset.transpose();//(I -KH)P(I - KH)' + KRK'// Handle wrapping of angleswrapStateAngles();FB_DEBUG("Kalman gain subset is:\n" <<kalman_gain_subset << "\nInnovation is:\n" <<innovation_subset << "\nCorrected full state is:\n" <<state_ << "\nCorrected full estimate error covariance is:\n" <<estimate_error_covariance_ <<"\n\n---------------------- /Ekf::correct ----------------------\n");}
}
//下面是扩展卡尔曼滤波的时间更新
void Ekf::predict(const rclcpp::Time & reference_time,const rclcpp::Duration & delta)
{const double delta_sec = filter_utilities::toSec(delta);//设置离散时间间隔FB_DEBUG("---------------------- Ekf::predict ----------------------\n" <<"delta is " << filter_utilities::toSec(delta) << "\n" <<"state is " << state_ << "\n");double roll = state_(StateMemberRoll);double pitch = state_(StateMemberPitch);double yaw = state_(StateMemberYaw);double x_vel = state_(StateMemberVx);double y_vel = state_(StateMemberVy);double z_vel = state_(StateMemberVz);double pitch_vel = state_(StateMemberVpitch);double yaw_vel = state_(StateMemberVyaw);double x_acc = state_(StateMemberAx);double y_acc = state_(StateMemberAy);double z_acc = state_(StateMemberAz);// We'll need these trig calculations a lot.double sp = ::sin(pitch);double cp = ::cos(pitch);double cpi = 1.0 / cp;double tp = sp * cpi;double sr = ::sin(roll);double cr = ::cos(roll);double sy = ::sin(yaw);double cy = ::cos(yaw);prepareControl(reference_time, delta);//X是在世界坐标系下，X的一阶导和二阶导是在载体坐标系下，因此需要根据k-1时刻的姿态转换到世界坐标系下// Prepare the transfer functiontransfer_function_(StateMemberX, StateMemberVx) = cy * cp * delta_sec;transfer_function_(StateMemberX, StateMemberVy) =(cy * sp * sr - sy * cr) * delta_sec;transfer_function_(StateMemberX, StateMemberVz) =(cy * sp * cr + sy * sr) * delta_sec;transfer_function_(StateMemberX, StateMemberAx) =0.5 * transfer_function_(StateMemberX, StateMemberVx) * delta_sec;transfer_function_(StateMemberX, StateMemberAy) =0.5 * transfer_function_(StateMemberX, StateMemberVy) * delta_sec;transfer_function_(StateMemberX, StateMemberAz) =0.5 * transfer_function_(StateMemberX, StateMemberVz) * delta_sec;transfer_function_(StateMemberY, StateMemberVx) = sy * cp * delta_sec;transfer_function_(StateMemberY, StateMemberVy) =(sy * sp * sr + cy * cr) * delta_sec;transfer_function_(StateMemberY, StateMemberVz) =(sy * sp * cr - cy * sr) * delta_sec;transfer_function_(StateMemberY, StateMemberAx) =0.5 * transfer_function_(StateMemberY, StateMemberVx) * delta_sec;transfer_function_(StateMemberY, StateMemberAy) =0.5 * transfer_function_(StateMemberY, StateMemberVy) * delta_sec;transfer_function_(StateMemberY, StateMemberAz) =0.5 * transfer_function_(StateMemberY, StateMemberVz) * delta_sec;transfer_function_(StateMemberZ, StateMemberVx) = -sp * delta_sec;transfer_function_(StateMemberZ, StateMemberVy) = cp * sr * delta_sec;transfer_function_(StateMemberZ, StateMemberVz) = cp * cr * delta_sec;transfer_function_(StateMemberZ, StateMemberAx) =0.5 * transfer_function_(StateMemberZ, StateMemberVx) * delta_sec;transfer_function_(StateMemberZ, StateMemberAy) =0.5 * transfer_function_(StateMemberZ, StateMemberVy) * delta_sec;transfer_function_(StateMemberZ, StateMemberAz) =0.5 * transfer_function_(StateMemberZ, StateMemberVz) * delta_sec;transfer_function_(StateMemberRoll, StateMemberVroll) = delta_sec;transfer_function_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta_sec;transfer_function_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta_sec;transfer_function_(StateMemberPitch, StateMemberVpitch) = cr * delta_sec;transfer_function_(StateMemberPitch, StateMemberVyaw) = -sr * delta_sec;transfer_function_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta_sec;transfer_function_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta_sec;transfer_function_(StateMemberVx, StateMemberAx) = delta_sec;transfer_function_(StateMemberVy, StateMemberAy) = delta_sec;transfer_function_(StateMemberVz, StateMemberAz) = delta_sec;// Prepare the transfer function Jacobian. This function is analytically// derived from the transfer function.//求解雅克比矩阵double x_coeff = 0.0;double y_coeff = 0.0;double z_coeff = 0.0;double one_half_at_squared = 0.5 * delta_sec * delta_sec;y_coeff = cy * sp * cr + sy * sr;z_coeff = -cy * sp * sr + sy * cr;double dFx_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared;double dFR_dR = 1.0 + (cr * tp * pitch_vel - sr * tp * yaw_vel) * delta_sec;x_coeff = -cy * sp;y_coeff = cy * cp * sr;z_coeff = cy * cp * cr;double dFx_dP =(x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *one_half_at_squared;double dFR_dP =(cpi * cpi * sr * pitch_vel + cpi * cpi * cr * yaw_vel) * delta_sec;x_coeff = -sy * cp;y_coeff = -sy * sp * sr - cy * cr;z_coeff = -sy * sp * cr + cy * sr;double dFx_dY =(x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *one_half_at_squared;y_coeff = sy * sp * cr - cy * sr;z_coeff = -sy * sp * sr - cy * cr;double dFy_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared;double dFP_dR = (-sr * pitch_vel - cr * yaw_vel) * delta_sec;x_coeff = -sy * sp;y_coeff = sy * cp * sr;z_coeff = sy * cp * cr;double dFy_dP =(x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *one_half_at_squared;x_coeff = cy * cp;y_coeff = cy * sp * sr - sy * cr;z_coeff = cy * sp * cr + sy * sr;double dFy_dY =(x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *one_half_at_squared;y_coeff = cp * cr;z_coeff = -cp * sr;double dFz_dR = (y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(y_coeff * y_acc + z_coeff * z_acc) * one_half_at_squared;double dFY_dR = (cr * cpi * pitch_vel - sr * cpi * yaw_vel) * delta_sec;x_coeff = -cp;y_coeff = -sp * sr;z_coeff = -sp * cr;double dFz_dP =(x_coeff * x_vel + y_coeff * y_vel + z_coeff * z_vel) * delta_sec +(x_coeff * x_acc + y_coeff * y_acc + z_coeff * z_acc) *one_half_at_squared;double dFY_dP =(sr * tp * cpi * pitch_vel + cr * tp * cpi * yaw_vel) * delta_sec;// Much of the transfer function Jacobian is identical to the transfer// function 计算雅克比矩阵，其为线性化的状态转移矩阵transfer_function_jacobian_ = transfer_function_;transfer_function_jacobian_(StateMemberX, StateMemberRoll) = dFx_dR;transfer_function_jacobian_(StateMemberX, StateMemberPitch) = dFx_dP;transfer_function_jacobian_(StateMemberX, StateMemberYaw) = dFx_dY;transfer_function_jacobian_(StateMemberY, StateMemberRoll) = dFy_dR;transfer_function_jacobian_(StateMemberY, StateMemberPitch) = dFy_dP;transfer_function_jacobian_(StateMemberY, StateMemberYaw) = dFy_dY;transfer_function_jacobian_(StateMemberZ, StateMemberRoll) = dFz_dR;transfer_function_jacobian_(StateMemberZ, StateMemberPitch) = dFz_dP;transfer_function_jacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR;transfer_function_jacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP;transfer_function_jacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR;transfer_function_jacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR;transfer_function_jacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP;FB_DEBUG("Transfer function is:\n" <<transfer_function_ << "\nTransfer function Jacobian is:\n" <<transfer_function_jacobian_ << "\nProcess noise covariance is:\n" <<process_noise_covariance_ << "\nCurrent state is:\n" <<state_ << "\n");//过程噪声协方差矩阵Q位于filter_base.cppEigen::MatrixXd * process_noise_covariance = &process_noise_covariance_;if (use_dynamic_process_noise_covariance_) {computeDynamicProcessNoiseCovariance(state_);process_noise_covariance = &dynamic_process_noise_covariance_;}// (1) Apply control terms, which are actually accelerations预测状态向量的状态转移矩阵state_(StateMemberVroll) +=control_acceleration_(ControlMemberVroll) * delta_sec;state_(StateMemberVpitch) +=control_acceleration_(ControlMemberVpitch) * delta_sec;state_(StateMemberVyaw) +=control_acceleration_(ControlMemberVyaw) * delta_sec;state_(StateMemberAx) = (control_update_vector_[ControlMemberVx] ?control_acceleration_(ControlMemberVx) :state_(StateMemberAx));state_(StateMemberAy) = (control_update_vector_[ControlMemberVy] ?control_acceleration_(ControlMemberVy) :state_(StateMemberAy));state_(StateMemberAz) = (control_update_vector_[ControlMemberVz] ?control_acceleration_(ControlMemberVz) :state_(StateMemberAz));// (2) Project the state forward: x = Ax + Bu (really, x = f(x, u))state_ = transfer_function_ * state_;// Handle wrappingwrapStateAngles();FB_DEBUG("Predicted state is:\n" <<state_ << "\nCurrent estimate error covariance is:\n" <<estimate_error_covariance_ << "\n");// (3) Project the error forward: P = J * P * J' + Q 预测其噪声协方差矩阵estimate_error_covariance_ =(transfer_function_jacobian_ * estimate_error_covariance_ *transfer_function_jacobian_.transpose());estimate_error_covariance_.noalias() +=delta_sec * (*process_noise_covariance);FB_DEBUG("Predicted estimate error covariance is:\n" <<estimate_error_covariance_ <<"\n\n--------------------- /Ekf::predict ----------------------\n");
}}  // namespace robot_localization

robot_localization中EKF源码介绍相关推荐

c语言复制粘贴源码,c语言函数memccpy()如何复制内存中的内容实例源码介绍
c语言函数memccpy()如何复制内存中的内容实例源码介绍.引入的头文件:#include memccpy()函数定义:void * memccpy(void *dest, const void * ...
4- vue django restful framework 打造生鲜超市 -restful api 与前端源码介绍
使用Python3.6与Django2.0.2(Django-rest-framework)以及前端vue开发的前后端分离的商城网站项目支持支付宝支付(暂不支持微信支付),支持手机短信验证码注册, ...
c语言临时文件作用,c语言函数mktemp()产生唯一临时文件名实例源码介绍
c语言函数mktemp()产生唯一临时文件名实例源码介绍.有关的函数:tmpfile引入的头文件:#include 定义函数mktemp():char * mktemp(char * template ...
linux ssh rpm包,RHEL6(CentOS6)中使用源码包编译生成RPM的基本方法：升级OpenSSH篇
RHEL6(CentOS6)中使用源码包编译生成RPM的基本方法:升级OpenSSH篇具体过程请见代码~ # cp openssh-7.1p1.tar.gz /root/rpmbuild/SOURC ...
linux部署3proxy源码,在CentOS 7系统中从源码安装RTPProxy的方法
本文介绍在CentOS 7操作系统中从源码安装RTPProxy的方法,按照以下步骤操作即可成功. 在CentOS 7.x上安装RTPProxy 1.将目录更改为/usr/src: [root@kama ...
Weka开发［８］－ID3源码介绍
为什么80%的码农都做不了架构师?>>> 这次介绍一下Id3源码,这次用Weka的源码介绍一下.首先Id3是继承于Classifier的: public class Id3 e ...
Linux 内核中RAID5源码详解之守护进程raid5d
Linux 内核中RAID5源码详解之守护进程raid5d 对于一个人,大脑支配着他的一举一动:对于一支部队,指挥中心控制着它的所有活动:同样,对于内核中的RAID5,也需要一个像大脑一样的东西来支配 ...
Live555源码阅读笔记（一）：源码介绍文档及源码目录结构
目录一.Live555介绍 1.Live555项目介绍 2.官网及帮助文档介绍二.源码目录结构 1.UsageEnvironment 2.BasicUsageEnvironment 3.group ...
Fabric中PBFT源码解读——Checkpoint机制
文章目录 1. 写在前面 1.1 前置阅读 1.2 对TestCheckpoint函数的测试 2. 对TestCheckpoint函数运行流程的解读 2.1 Checkpoint和Water mark ...

robot_localization中EKF源码介绍

robot_localization中EKF源码介绍

robot_localization中EKF源码介绍相关推荐

最新文章

热门文章