Pixhawk Lacal_position_estimator Data stream

Lacal_position_estimator and Position_estimator_inav are side-by-side relationships for position estimation

The specific selection process should be like this, not yet tested, for reference only

1.cmake compiles the lacal_position_estimator into. Px4

Start Lacal_position_estimator in 2.rcS, make sys_mc_est_group=1, or comment out the judgment

Second, the processing of data flow, taking the optical flow as an example

Because Lacal_position_estimator is a Kalman algorithm, the author of this aspect is relatively weak, can only be sorted out the data flow, the specific physical meaning still do not know

int local_position_estimator_thread_main (int argc, char *argv[]) {WARNX ("starting"); using namespace control; Blocklocalpositionestimator est;thread_running = True;while (!thread_should_exit) {est.update ();} Warnx ("exiting."); Thread_running = False;return 0;}

Jump to Est.update ();

void Blocklocalpositionestimator::update ()

The following works in this cycle

while (!thread_should_exit) {est.update ();}

1. data sources:

See which updates is available//subscription data bool flowupdated = _sub_flow.updated (); bool paramsupdated = _SUB_PARAM_UPDATE.UPD Ated (); bool baroupdated = _sub_sensor.updated (); bool gpsupdated = _gps_on.get () && _sub_gps.updated (); bool homeupdated = _sub_home.updated (), bool visionupdated = _vision_on.get () && _sub_vision_pos.updated (); bool mocapupdated = _sub_mocap.updated (); bool lidarupdated = (_sub_lidar! = NULL) && _sub_lidar->updated (); bool sonarupdated = (_sub_sonar! = NULL) && _sub_sonar->updated ();

Get new data//Gets the data updatesubscriptions ();

Its original function is

virtual void Updatesubscriptions () {block::updatesubscriptions (); if (GetChildren (). GetHead () = NULL) { Updatechildsubscriptions (); }}

Jump to Block::updatesubscriptions ();

void Block::updatesubscriptions () {Uorb::subscriptionnode *sub = GetSubscriptions (). GetHead (); int count = 0;while (sub!) = NULL) {if (count++ > Maxsubscriptionsperblock) {char name[blocknamelengthmax];getname (name, Blocknamelengthmax); printf ("Exceeded max subscriptions for block:%s\n", name); Sub->update (); sub = Sub->getsibling ();}}

Jump to Sub->update ();

void subscriptionbase::update (void *data) {if (Updated ()) {int ret = orb_copy (_meta, _handle, data); if (ret! = PX4_OK) {WA RNX ("Orb copy Failed"); }}}

The subject of the subscription is then copied by using the while (sub! = NULL) {} loop.

2. Status Forecast

Kalman Matrix

_x (), state vector

_u (), input vector

_p (), State covariance matrix

2.1 Initial value (run only once)

INITP (); _x.setzero (); _u.setzero (); _flowx = 0;_flowy = 0; Matrix<float, n_x, n_x>  _p;//State covariance matrixvoid BLOCKLOCALPOSITIONESTIMATOR::INITP () {_P.setZero ( ); _p (x_x, x_x) = 1;_p (x_y, x_y) = 1;_p (X_z, x_z) = 1;_p (X_VX, X_VX) = 1;_p (X_vy, X_vy) = 1;_p (X_vz, X_vz) = 1;_p (X_BX, X_b) x) = 1e-6;_p (x_by, x_by) = 1e-6;_p (X_BZ, X_BZ) = 1e-6;_p (X_tz, X_tz) = 1;}

2.2 The matrix or vector required to prepare the operation

Input vector _u

<pre name= "code" class= "CPP" >if (_integrate.get () && _sub_att.get (). R_valid) {matrix3f R_att (_sub_att.get (). R); vector3f A (_sub_sensor.get (). accelerometer_m_s2) _u = R_att * A;_u (U_az) + = 9.81f; Add g}

Dynamic matrix A

Dynamics Matrixmatrix<float, n_x, n_x>  A;//State Dynamics Matrixa.setzero ();//derivative of position are ve Locitya (x_x, X_VX) = 1; A (x_y, x_vy) = 1; A (x_z, x_vz) = 1;//derivative of velocity is accelerometer acceleration//(in input matrix)-bias (in body frame) Matrix3 F R_att (_sub_att.get (). R); A (X_VX, X_BX) =-r_att (0, 0); A (X_VX, x_by) =-r_att (0, 1); A (X_VX, X_BZ) =-r_att (0, 2); A (X_vy, X_BX) =-r_att (1, 0); A (X_vy, x_by) =-r_att (1, 1); A (X_vy, X_BZ) =-r_att (1, 2); A (X_vz, X_BX) =-r_att (2, 0); A (X_vz, x_by) =-r_att (2, 1); A (X_vz, X_BZ) =-r_att (2, 2);

Input Matrix B

Input Matrix<span style= "White-space:pre" ></span>matrix<float, n_x, n_u>  B;//input Matrixb.setzero (); B (X_VX, u_ax) = 1; B (X_vy, u_ay) = 1; B (X_vz, U_az) = 1;

Input noise covariance matrix R

Input noise covariance matrixmatrix<float, N_u, n_u> R; R.setzero (); R (U_ax, U_ax) = _accel_xy_stddev.get () * _accel_xy_stddev.get (); R (U_ay, U_ay) = _accel_xy_stddev.get () * _accel_xy_stddev.get (); R (U_az, U_az) = _accel_z_stddev.get () * _accel_z_stddev.get ();

System process Noise Matrix Q

Process noise power matrixmatrix<float, n_x, n_x>  Q; Q.setzero (); Float pn_p_sq = _pn_p_noise_density.get () * _pn_p_noise_density.get (); Float pn_v_sq = _pn_v_noise_ Density.get () * _pn_v_noise_density.get (); Q (x_x, x_x) = PN_P_SQ; Q (x_y, x_y) = PN_P_SQ; Q (x_z, x_z) = PN_P_SQ; Q (X_VX, X_VX) = PN_V_SQ; Q (X_vy, x_vy) = PN_V_SQ; Q (X_vz, X_VZ) = pn_v_sq;//Technically, the noise is in the body frame,//and the components were all the same, so//Ignori ng for nowfloat pn_b_sq = _pn_b_noise_density.get () * _pn_b_noise_density.get (); Q (X_BX, x_bx) = PN_B_SQ; Q (x_by, x_by) = PN_B_SQ; Q (X_BZ, X_BZ) = pn_b_sq;//Terrain random walk noisefloat pn_t_sq = _pn_t_noise_density.get () * _pn_t_noise_density.get (); Q (X_tz, X_tz) = PN_T_SQ;

Continuous-time Kalman filter Predictive value

Continuous time Kalman filter predictionvector<float, n_x> dx = (A * _x + B * _u) * GETDT ();

With the new-x,-p.

propagate_x + = DX; State vector _p + = (A * _p + _p * a.transpose () +b * R * b.transpose () + Q) * GETDT ();//State covariance matrix

The input is a custom noise matrix, a rotation matrix, and an acceleration vector, and the output is -X ( state vector ) , -P ( State covariance matrix )

3 . Corrective Correct

Flowcorrect ();

Measurement Matrix C

Flow measurement matrix and noise matrixmatrix<float, N_y_flow, n_x> C; C.setzero (); C (y_flow_x, x_x) = 1; C (y_flow_y, x_y) = 1;

Noise Matrix R

<span style= "White-space:pre" ></span>r.setzero (); R (y_flow_x, y_flow_x) =_flow_xy_stddev.get () * _flow_xy_stddev.get (); R (y_flow_y, y_flow_y) =_flow_xy_stddev.get () * _flow_xy_stddev.get ();

Residual vector r

Vector<float, 2> r = y-c * _x; (_x from Above sonarcorrect (), etc.)

residual covariance (inverse)

residual covariance, (inverse) matrix<float, N_y_flow, n_y_flow> s_i =inv<float, n_y_flow> (C * _P * C.TRANSP OSE () + R);(_p from the above sonarcorrect (), etc.)

Fault detection

Fault Detectionfloat beta = (r.transpose () * (s_i * r)) (0, 0), if (Beta > Beta_table[n_y_flow]) {if (_flowfault < Fault_minor) {//mavlink_and_console_log_info (&mavlink_log_pub, "[Lpe] Flow FAULT,  beta%5.2f", double (Beta)) ; _flowfault = Fault_minor;}} else if (_flowfault) {_flowfault = Fault_none;//mavlink_and_console_log_info (&mavlink_log_pub, "[Lpe] flow OK");}

Optical flow correction

if (_flowfault < fault_lvl_disable) {matrix<float, n_x, n_y_flow> K =_p * c.transpose () * s_i;_x + = K * r;    With the new _x, the state vector _p-= K * C * _p;  With the new _p, the state covariance matrix} else {/reset flow integral to the current estimate of position//if a fault occurred_flowx = _x (x_x); _flowy = _x (x_y);}

4. Release Status

if (_althomeinitialized) {//Update all publications if Possiblepublishlocalpos ();p ublishestimatorstatus (); if (_ canestimatexy) {Publishglobalpos ();}}

Publishlocalpos () released so many

_pub_lpos.get (). Timestamp = _timestamp;_pub_lpos.get (). Xy_valid = _canestimatexy;_pub_lpos.get (). Z_valid = _ Canestimatez;_pub_lpos.get (). V_xy_valid = _canestimatexy;_pub_lpos.get (). V_z_valid = _canestimatez;_pub_lpos.get () . x = _x (x_x);  North_pub_lpos.get (). y = _x (x_y); East_pub_lpos.get (). z = _x (x_z);  Down_pub_lpos.get (). VX = _x (X_VX);  North_pub_lpos.get (). VY = _x (X_vy); East_pub_lpos.get (). VZ = _x (X_VZ); Down_pub_lpos.get (). Yaw = _sub_att.get (). Yaw;_pub_lpos.get (). Xy_global = _sub_home.get (). Timestamp! = 0; Need home for Reference_pub_lpos.get (). Z_global = _baroinitialized;_pub_lpos.get (). Ref_timestamp = _sub_home.get (). Timestamp;_pub_lpos.get (). Ref_lat = _map_ref.lat_rad * 180/m_pi;_pub_lpos.get (). Ref_lon = _map_ref.lon_rad * 180/M_PI; _pub_lpos.get (). Ref_alt = _sub_home.get (). Alt;_pub_lpos.get (). Dist_bottom = AGL (); _pub_lpos.get (). dist_bottom_rate =-_x (X_VZ); _pub_lpos.get (). Surface_bottom_timestamp = _timestamp;_pub_lpos.get (). Dist_bottom_valid = _canEstiMatez;_pub_lpos.get (). Eph = Sqrtf (_p (x_x, x_x) + _p (x_y, x_y)); _pub_lpos.get (). EPV = Sqrtf (_p (X_z, x_z)); 

Publishestimatorstatus () publishes so much

_pub_est_status.get (). Timestamp = _timestamp;for (int i = 0; i < n_x; i++) {_pub_est_status.get (). States[i] = _x (i); _pu B_est_status.get (). Covariances[i] = _p (i, i);} _pub_est_status.get (). N_states = N_x;_pub_est_status.get (). Nan_flags = 0;_pub_est_status.get (). Health_flags = ((_ Barofault > Fault_lvl_disable) << Sensor_baro) + ((_gpsfault > Fault_lvl_disable) << Sensor_gps) + ((_ Lidarfault > Fault_lvl_disable) << Sensor_lidar) + ((_flowfault > Fault_lvl_disable) << sensor_flow) + ( (_sonarfault > Fault_lvl_disable) << sensor_sonar) + ((_visionfault > Fault_lvl_disable) << sensor_ VISION) + ((_mocapfault > Fault_lvl_disable) << sensor_mocap); _pub_est_status.get (). Timeout_flags = (_ baroinitialized << Sensor_baro) + (_gpsinitialized << Sensor_gps) + (_flowinitialized << sensor_flow) + (_lidarinitialized << Sensor_lidar) + (_sonarinitialized << Sensor_sonar) + (_visioninitialized << sensor_vision) + (_mocapInitialized << Sensor_mocap); 

Publishglobalpos () released so many

_pub_gpos.get (). Timestamp = _timestamp;_pub_gpos.get (). Time_utc_usec = _sub_gps.get (). Time_utc_usec;_pub_gpos.get ( ). lat = Lat;_pub_gpos.get (). Lon = Lon;_pub_gpos.get (). alt = Alt;_pub_gpos.get (). Vel_n = _x (X_VX); _pub_gpos.get (). vel_e = _x (X_vy); _pub_gpos.get (). Vel_d = _x (X_VZ), _pub_gpos.get (). Yaw = _sub_att.get (). Yaw;_pub_gpos.get (). Eph = Sqrtf (_P (x _x, x_x) + _p (x_y, x_y)); _pub_gpos.get (). EPV = Sqrtf (_p (X_z, x_z)); _pub_gpos.get (). Terrain_alt = _althome-_x (X_tz); _pub _gpos.get (). Terrain_alt_valid = _canestimatet;_pub_gpos.get (). dead_reckoning =!_canestimatexy &&!_ Xytimeout;_pub_gpos.get (). Pressure_alt = _sub_sensor.get (). Baro_alt_meter[0];

Pixhawk Lacal_position_estimator Data stream