position estimator inav需要算法進行高度估計

因為單純的acc.z或者超聲波/氣壓計都不能很好的估計實際飛行器高度。加速度高頻，氣壓計會溫漂，超說里面有2次矯正。floatz_est[20.0f0.0f};//posvelfloatz_est[2]0.0f0.0fzfloatacc[]0.0f0.0f0.0f地理坐標系（NED）的加速度數據floatacc_bias0.0f0.0f0.0f機體坐標系下的加速度偏移量floatcorr_baro=0.0f;//氣壓計校正系數thread_running=true;while(wait_baro&&!thread_should_exit){if(ret<0){/*pollerror}elseif(ret>0) bined_sub,if if}}elsebaro_offset/=(float) }}}}}，，sensor.accelerometer_m_s2[0acc_bias[0];sensor.accelerometer_m_s2[2acc_bias[2];for(inti=0;i<3;{acc[i]=for(intj=0;j<3;j++)acc[i]+=PX4_R(att.R,i,j)*}}//for(inti=0;i<3;{floatc=for(intj=0;j<3;j++)}}}{{acc=}x[0]x[1]*dtacc*dt*dt2.0f;//位置x[1]+=acc*dt;//速度}}voidinertial_filter_correct(floate,floatdt,floatx[2],inti,floatw){floatewdt=e*w*dt;//corr_baro*params.w_z_baro*if(i==0){}}}lidar_first=lidar_offset=dist_ground+}corr_lidar=lidar_offset-dist_ground-accel_bias_corr[2]-=corr_lidar*params.w_z_lidar*}else}正傳感器對應能感知的量，比如光流就是感知速度，GPS可以感知位置和速度。1.1.floatz_est[20.0f,0.0fz3.floatacc_bias0.0f0.0f0.0f5.floatcorr_baro=//{0.0f,0.0f//N(pos,{0.0f,0.0f//D(pos,11.floatcorr_vision[3][2]= {0.0f,0.0f//E(pos,{0.0f//N16.floatcorr_mocap[3][1]=//D(pos,{0.0f,0.0f//N(pos,{0.0f,0.0f//E(pos,{0.0f,0.0f6.floatcorr_gps[3][2]=2.floatacc0.0f0.0f,0.0f//地理坐標系（NED） {0.0f //E {0.0f //D21floatcorr_lidar0.0f;//22.floatcorr_flow[]={0.0f,0.0f};//N24.boolgps_valid= //GPSis25.boollidar_valid= //lidaris26.boolflow_valid= //flowis27.boolflow_accurate= //flowshouldbeaccurate(thisflagnotupdatedifflow_valid28.boolvision_valid= //visionis29.boolmocap_valid= //mocapis2.baro_offset/=2.baro_offset/= corr_baro=baro_offset-sensor.baro_alt_meter[0]-corr_lidar=lidar_offset-dist_ground-corr_flow[0]=flow_v[0]-x_est[1];/*velocitycorrectioncorr_flow[1]=flow_v[1]-corr_vision[0][0]=vision.x-x_est[0];/*calculatecorrectionforpositioncorr_vision[1][0]=vision.y-corr_vision[2][0]=vision.z-corr_vision[0][1]=vision.vx-x_est[1];/*calculatecorrectionforvelocitycorr_vision[1][1]=vision.vy-corr_vision[2][1]=vision.vz-corr_mocap[0][0]=mocap.x-x_est[0];/*calculatecorrectionforpositioncorr_mocap[1][0]=mocap.y-corr_mocap[2][0]=mocap.z-corr_gps[0][0]=gps_proj[0]-est_buf[est_i][0][0];/*calculatecorrectionforpositioncorr_gps[1][0]=gps_proj[1]-corr_gps[2][0]=local_pos.ref_alt-alt-corr_gps[0][1]=gps.vel_n_m_s-est_buf[est_i][0][1];/*calculatecorrectionforvelocitycorr_gps[1][1]=gps.vel_e_m_s-corr_gps[2][1]=gps.vel_d_m_s-w_gps_xy=min_eph_epv/fmaxf(min_eph_epv,w_gps_z=min_eph_epv/fmaxf(min_eph_epv,if((flow_valid||lidar_valid)&&t>(flow_time+2.if(gps_valid&&(t>(gps.timestamp_position+3.if(vision_valid&&(t>(vision.timestamp_boot+4.if(mocap_valid&&(t>(mocap.timestamp_boot+5.if(lidar_valid&&(t>(lidar_time+1.1./*useGPSifit'svalidandreferencepositioninitialized3.booluse_gps_z=ref_inited&&gps_valid&¶ms.w_z_gps_p>5.booluse_vision_xy=vision_valid&¶ms.w_xy_vision_p>7./*useMOCAPifit'svalidandhasavalidweightparameter9.if(params.disable_mocap){//disablemocapiffakegpsis13.booluse_flow=flow_valid&&(flow_accurate||15.use_lidar=lidar_valid&&14./*useLIDARifit'svalidandlidaraltitudeestimationisenabled12./*useflowifit'svalidand(accurateornoGPSavailable)use_mocap=8.booluse_mocap=mocap_valid&¶ms.w_mocap_p>MIN_VALID_W&¶ms.att_ext_hdg_m==ading;//checkifexternalheadingis6.booluse_vision_z=vision_valid&¶ms.w_z_vision_p>4./*useVISIONifit'svalidandhasavalidweightparameter2.booluse_gps_xy=ref_inited&&gps_valid&¶ms.w_xy_gps_p>floatflow_q=flow.quality/2.floatflow_q_weight=(flow_q-params.flow_q_min)/(1.0f-3.w_flow=PX4_R(att.R,2,2)*flow_q_weight/fmaxf(1.0f,4.if(!flow_accurate) w_flow*=6.8.floatw_xy_gps_p=params.w_xy_gps_p*9.floatw_xy_gps_v=params.w_xy_gps_v*10.floatw_z_gps_p=params.w_z_gps_p*11.floatw_z_gps_v=params.w_z_gps_v*13.floatw_xy_vision_p=14.floatw_xy_vision_v=15.floatw_z_vision_p=17.floatw_mocap_p=18./*reduceGPSweightifopticalflowisgood19.if(use_flow&&flow_accurate) w_xy_gps_p*= w_xy_gps_v*=23./*barooffsetcorrection24.if(use_gps_z) floatoffs_corr=corr_gps[2][0]*w_z_gps_p* baro_offset+= corr_baro+=29./*accelerometerbiascorrectionforGPS(usebufferedrotationmatrix)30.floataccel_bias_corr[3]={0.0f,0.0f,0.0fif(use_gps_xy)corr_gps[0][0]w_xy_gps_p*corr_gps[0][1]corr_gps[1][0]w_xy_gps_p*corr_gps[1][1]}if(use_gps_z)corr_gps[2][0]w_z_gps_p*corr_gps[2][1]}/*transformerrorfromNEDframebodyframefor(inti=0;i<3;i++)floatc=0.0f;for(intj=0;j<3;j++)c+=R_gps[j][i]* if(PX4_ISFINITE(c))acc_bias[i]+=c*params.w_acc_bias* /*accelerometerbiascorrectionforVISION(usebufferedrotationmatrix)accel_bias_corr[0]=accel_bias_corr[1]=accel_bias_corr[2]=0.0f;if(use_vision_xy)accel_bias_corr[0]-=corr_vision[0][0]*w_xy_vision_p*accel_bias_corr[0]-=corr_vision[0][1]*]accel_bias_corr[1]-=corr_vision[1][0*w_xy_vision_p*]accel_bias_corr[1]-=corr_vision[1][1]*if(use_vision_z)accel_bias_corr[2]-=corr_vision[2][0]*w_z_vision_p*/*accelerometerbiascorrectionforMOCAP(usebufferedrotationmatrix)accel_bias_corr[0]=accel_bias_corr[1]=accel_bias_corr[2]=0.0f;if(use_mocap)accel_bias_corr[0]-=corr_mocap[0][0]*w_mocap_p*accel_bias_corr[1]-=corr_mocap[1][0]*w_mocap_p*accel_bias_corr[2]-=corr_mocap[2][0]*w_mocap_p*/*transformerrorvectorfromNEDframetobodyframefor(inti=0;i<3;i++)floatc=0.0f;for(intj=0;j<3;j++)c+=PX4_R(att.R,j,i)* if(PX4_ISFINITE(c))acc_bias[i]+=c*params.w_acc_bias* /*accelerometerbiascorrectionforflowandbaro(assumethatthereisnodelay)accel_bias_corr[0]=accel_bias_corr[1]=accel_bias_corr[2]=0.0f;if(use_flow)accel_bias_corr[0]-=corr_flow[0]*accel_bias_corr[1]-=corr_flow[1]*if(use_lidar)accel_bias_corr[2]-=corr_lidar*params.w_z_lidar*}elseaccel_bias_corr[2]-=corr_baro*params.w_z_baro*82./*transformerrorvector83.for(inti=0;i<3;i++) floatc= for(intj=0;j<3;j++) c+=PX4_R(att.R,j,i)* if(PX4_ISFINITE(c)) acc_bias[i]+=c*params.w_acc_bias* 1.1./*sensorcombinedbined_sub,if(sensor.accelerometer_timestamp[0]!=accel_timestamp)/*correctaccelbiassensor.accelerometer_m_s2[1]-=for(inti=0;i<3;i++)acc[i]+=PX4_R(att.R,i,j)*}acc[2]+=}else}accel_timestamp=memset(acc,0,}for(intj=0;j<3;j++)acc[i]=/*transformaccelerationvectorfrombodyframetoNEDframesensor.accelerometer_m_s2[2]-=sensor.accelerometer_m_s2[0]-=if(att.R_valid)4.if(updated)bined_sub,2.}2.if(can_estimate_xy)4.inertial_filter_predict(dt,x_est,2.if(can_estimate_xy)4.inertial_filter_predict(dt,x_est,6.3.5.inertial_filter_predict(dt,y_est,7.inertial_filter_predict(dt,z_est,1.voidinertial_filter_predict(floatdt,floatx[2],float2. if(isfinite(dt)) if(!isfinite(acc))acc=} x[0]+=x[1]*dt+acc*dt*dt/ x[1]+=acc*}/*inertialfiltercorrectionforaltitude2.if(use_lidar) 4.}else inertial_filter_correct(corr_baro,dt,z_est,0,6.7.if(use_gps_z) epv=fminf(epv, inertial_filter_correct(corr_gps[2][0],dt,z_est,0, inertial_filter_correct(corr_gps[2][1],dt,z_est,1,12.if(use_vision_z) epv=fminf(epv, inertial_filter_correct(corr_vision[2][0],dt,z_est,0,16.if(use_mocap) epv=fminf(epv, inertial_filter_correct(corr_mocap[2][0],dt,z_est,0,20.if(can_estimate_xy) /*inertialfiltercorrectionforposition if(use_flow) eph=fminf(eph, inertial_filter_correct(corr_flow[0],dt,x_est,1,params.w_xy_flow* inertial_filter_correct(corr_flow[1],dt,y_est,1,params.w_xy_flow* if(use_gps_xy) eph=fminf(eph, inertial_filter_correct(corr_gps[0][0],dt,x_est,0, inertial_filter_correct(corr_gps[1][0],dt,y_est,0, if(gps.vel_ned_valid&&t<gps.timestamp_velocity+gps_topic_timeout) inertial_filter_correct(corr_gps[0][1],dt,x_est,1, inertial_filter_correct(corr_gps[1][1],dt,y_est,1, if(use_vision_xy) eph=fminf(eph, inertial_filter_correct(corr_vision[0][0],dt,x_est,0, inertial_filter_correct(corr_vision[1][0],dt,y_est,0, if(w_xy_vision_v>MIN_VALID_W) inertial_filter_correct(corr_vision[0][1],dt,x_est,1, inertial_filter_correct(corr_vision[1][1],dt,y_est,1, if(use_mocap) eph=fminf(eph, inertial_filter_correct(corr_mocap[0][0],dt,x_est,0, inertial_filter_correct(corr_mocap[1][0],dt,y_est,0, 51./*runterrainestimator52.terrain_estimator.predict(dt,&att,&sensor,2.e是修正系數；2.e是修正系數；dt周期時間；x[2]2float型成員的數組，x[0]是位置，x[1]3.i表示修正是位置還是速度，0是修正位置，1是修正速度；wvoidinertial_filter_correct(floate,floatdt,floatx[2],inti,float{if(isfinite(e)&&isfinite(w)&&isfinite(dt))floatewdt=e*w*dt;x[i]+=ewdt;if(i==0)x[1]+=w*}}/*publishlocalposition2.local_pos.xy_valid=3.local_pos.v_xy_valid=4.local_pos.xy_global=local_pos.xy_valid&&5.local_pos.z_global=local_pos.z_valid&&6.local_pos.x=7.local_pos.vx=8.local_pos.y=9.local_pos.vy=10.local_pos.z=11.local_pos.vz=12.local_pos.yaw=13.local_pos.dist_bottom_valid=14.local_pos.eph=15.local_pos.epv=17.if(local_pos.dist_bottom_valid) local_pos.dist_bottom= local_pos.dist_bottom_rate=-22.local_pos.timestamp=26.if(local_pos.xy_global&&local_pos.z_global) /*publishglobalposition global_pos.timestamp= global_pos.time_utc_usec= doubleest_lat, map_projection_reproject(&ref,local_pos.x,local_pos.y,&est_lat,global_pos.lat=global_pos.lon=global_pos.alt=local_pos.ref_alt-local_pos.z;global_pos.vel_n=global_pos.vel_e=global_pos.vel_d=local_pos.vz; global_pos.yaw=local_pos.yaw;global_pos.eph=global_pos.epv=epv;if(terrain_estimator.is_valid())global_pos.terrain_alt=global_pos.alt-global_pos.terrain_alt_valid=true;}elseglobal_pos.terrain_alt_valid= global_pos.pressure_alt=sensor.baro_alt_meter[0];if(vehicle_global_position_pub==NULL)vehicle_global_position_pub=orb_advertise(ORB_ID(vehicle_global_position),}else 1.1./*opticalfloworb_copy(ORB_ID(optical_flow),optical_flow_sub,flow_time=floatflow_q=flow.quality/4.if(updated&&lidar_valid)2.orb_check(optical_flow_sub, //qual=(uint8_t)(meancount*255/ floatdist_bottomlidar.current_distance;// //離地面距離>20cm&&flow.quality>某個值&&PX4_R(att.R2,20.7f(不知道什么意思 if(dist_bottom>flow_min_dist&&flow_q>params.flow_q_min&&PX4_R(att.R,2,2)>0.7f) /*distancetosurface //floatflow_dist=dist_bottom/PX4_R(att.R,2,2);//usethisifusing floatflow_dist=dist_bottom;//usethisifusing /*checkifflowiftoolargeforaccuratemeasurements /*calculateestimatedvelocityinbodyframe floatbody_v_est[2]={0.0f,0.0f for(inti=0;i<2;i++) body_v_est[i]=PX4_R(att.R,0,i)*x_est[1]+PX4_R(att.R,1,i)*y_est[1]+att.R,2,i)* /*setthisflagifflowshouldbeaccurateaccordingtocurrentvelocityandattitudeeestimate flow_accurate=fabsf(body_v_est[1]/flow_dist-att.rollspeed)<max_flow fabsf(body_v_est[0]/flow_dist+att.pitchspeed)< *xy軸機體速度/距離= /*calculateoffsetofflow-gyrousingalreadycalibratedgyrofrom flow_gyrospeed[0]=flow.gyro_x_rate_integral/(float)egration_timespan flow_gyrospeed[1]=flow.gyro_y_rate_integral/(float)egration_timespan flow_gyrospeed[2]=flow.gyro_z_rate_integral/(float)egration_timespan //moving if(n_flow>=100) gyro_offset_filtered[0]=flow_gyrospeed_filtered[0]- gyro_offset_filtered[1]=flow_gyrospeed_filtered[1]- gyro_offset_filtered[2]=flow_gyrospeed_filtered[2]- n_flow= flow_gyrospeed_filtered[0]=... flow_gyrospeed_filtered[1]= flow_gyrospeed_filtered[2]= att_gyrospeed_filtered[0]= att_gyrospeed_filtered[1]= att_gyrospeed_filtered[2]= }else flow_gyrospeed_filtered[0]=(flow_gyrospeed[0]+n_flow*/(n_flow+ flow_gyrospeed_filtered[1]=(flow_gyrospeed[1]+n_flow*/(n_flow+ flow_gyrospeed_filtered[2]=(flow_gyrospeed[2]+n_flow*/(n_flow+ att_gyrospeed_filtered[0]=(att.pitchspeed+n_flow*att_gyrospeed_filtered[0])/_flow+ att_gyrospeed_filtered[1]=(att.rollspeed+n_flow*att_gyrospeed_filtered[1])/flow+ att_gyrospeed_filtered[2]=(att.yawspeed+n_flow*att_gyrospeed_filtered[2])/low+ /*yawcompensation(flowsensorisnotincenterofrotation)->paramsin p[0]=-params.flow_module_offset_y*(flow_gyrospeed[2]-; p[1]=params.flow_module_offset_x*(flow_gyrospeed[2]- /*flow_gyrospeed[2]光流模塊上測得的z *gyro_offset_filtered[2]光流模塊上測得的z軸角速度平均值-飛控測得的z /*convertrawflowtoangularflow(rad/s) float /*checkforvehicleratessetpoint-itbelowthreshold->dontsubtract->better if doublerate_threshold= if(fabs(rates_setpoint.pitch)<rate_threshold)) 0f)*params.flow_k;//fornowtheflowhastobescaled(to else /* *accumulationtimespanin *pixflowmain.c *uin