本帖最后由 ariesys 于 2019-7-12 18:56 编辑
一、文件结构 精准降落模块位于Firmware/src/modules/landing_target_estimator文件夹，包含： Landing_target_estimator_main.cpp //主文件，入口文件 Landing_target_estimator_params.c //参数文件 LandingTargetEstimator.cpp //任务主文件，主流程，定义了LandingTargetEstimator类中的函数内容 LandingTargetEstimator.h //任务主文件，文件中定义了LandingTargetEstimator类 KalmanFilter.cpp //滤波文件，引用函数，定义了[size=13.3333px]KalmanFilter类成员函数 KalmanFilter.h //滤波文件，定义了KalmanFilter类： CMakeList.txt //编译文件
二、程序流程 在Landing_target_estimator_main.cpp中启动了进程，实例化了LandingTargetEstimator 类命名为est，在循环中持续调用est.update()函数，循环周期为： usleep(1000000 /landing_target_estimator_UPDATE_RATE_HZ);//50HZ 在LandingTargetEstimator.cpp文件中，定义了voidLandingTargetEstimator::update()函数内容： 进行kalman滤波器预测： //预测 _kalman_filter_x.predict(dt, -a(0), _params.acc_unc); _kalman_filter_y.predict(dt, -a(1), _params.acc_unc); 将irlock的值赋值给sensor_ra数组： sensor_ray(0) = - _irlockReport.pos_y; sensor_ray(1) = _irlockReport.pos_x;
乘上超声波的距离转换成位置误差： _rel_pos(0) = sensor_ray(0) / sensor_ray(2) * dist; _rel_pos(1) = sensor_ray(1) / sensor_ray(2) * dist; 再采用kalman滤波器更新： if (!_estimator_initialized) { //初始化 float vx_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vx : 0.f; float vy_init = _vehicleLocalPosition.v_xy_valid ? -_vehicleLocalPosition.vy : 0.f; _kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init); _kalman_filter_y.init(_rel_pos(1), vy_init, _params.pos_unc_init, _params.vel_unc_init); …… } else { //更新 bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist); bool update_y = _kalman_filter_y.update(_rel_pos(1), _params.meas_unc * dist * dist); …… } 在实例化LandingTargetEstimator类同时会实例化两个KalmanFilter类，命名为_kalman_filter_x、_kalman_filter_y，调用该类的构造函数。
//初始化 _kalman_filter_x.init(_rel_pos(0), vx_init, _params.pos_unc_init, _params.vel_unc_init); void KalmanFilter::init(float initial0, float initial1, float covInit00, float covInit11) { matrix::Vector<float, 2> initial; initial(0)= initial0; //相对位置 initial(1)= initial1; //速度 matrix::Matrix<float, 2, 2> covInit; covInit(0,0) = covInit00; //位置不确定性 covInit(1,1) = covInit11;//速度不确定性 init(initial,covInit);//初始化 }
//预测 _kalman_filter_x.predict(dt, -a(0), _params.acc_unc); void KalmanFilter::predict(float dt, float acc, float acc_unc) { _x(0) += _x(1) * dt + dt * dt / 2 * acc;//根据无人机的加速度、速度计算下一刻相对位置; _x(1) += acc * dt; //根据加速度计算下一刻速度; matrix::Matrix<float, 2, 2> A; // propagation matrix A(0,0) = 1; A(1,1) = 1; A(0,1) = dt; matrix::Matrix<float, 2, 1> G; // noise model G(0,0) = dt * dt / 2; G(1,0) = dt; //求过程噪声矩阵 matrix::Matrix<float, 2, 2> process_noise = G * G.transpose()* acc_unc; //协方差矩阵 _covariance = A * _covariance * A.transpose() + process_noise; } 程序中用到的状态方程为： 量测方程为： //更新 bool update_x = _kalman_filter_x.update(_rel_pos(0), _params.meas_unc * dist * dist); bool KalmanFilter::update(float meas, float measUnc) { // 计算量测误差 _residual = meas - _x(0); // H * P * H^T simply selectsP(0,0) _innovCov = _covariance(0, 0) + measUnc; // outlier rejection float beta = _residual / _innovCov * _residual; // 5% false alarm probability if (beta > 3.84f) { return false; } matrix::Vector<float, 2> kalmanGain; kalmanGain(0)= _covariance(0, 0); kalmanGain(1)= _covariance(1, 0); kalmanGain/= _innovCov; //Kalman增益系数 _x += kalmanGain * _residual;//状态更新 matrix::Matrix<float, 2, 2> identity; identity.identity(); matrix::Matrix<float, 2, 2> KH; // kalmanGain * H KH(0,0) = kalmanGain(0); KH(1,0) = kalmanGain(1); _covariance = (identity - KH) * _covariance;//重新加算协方差 return true; }
| 
发表于 2019-6-19 13:57:47
7074 浏览 8 回复
。