mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-12 07:13:33 +08:00
corrected errors in shear estimator module
This commit is contained in:
Marvin Harms
@@ -5,3 +5,5 @@ uint64 timestamp # time since system start (microseconds)
|
||||
float32[3] wind_estimate # WIND ESTIMATE IN ENU FRAME
|
||||
float32[3] wind_estimate_filtered # LP-FILTERED WIND ESTIMATE IN ENU FRAME, USED BY INDI CONTROLLER
|
||||
float32[3] position # position of the current estimate in the soaring frame
|
||||
|
||||
bool valid # tell, if estimate is valid for shear estimator
|
||||
|
||||
@@ -9,5 +9,12 @@ float32 by # wind offset in y-direction
|
||||
float32 h # vertical position of shear layer in soaring frame
|
||||
float32 a # shear strength
|
||||
|
||||
float32 sigma_vx # covariance of vx
|
||||
float32 sigma_vy # covariance of vy
|
||||
float32 sigma_bx # covariance of bx
|
||||
float32 sigma_by # covariance of by
|
||||
float32 sigma_h # covariance of h
|
||||
float32 sigma_a # covariance of a
|
||||
|
||||
bool params_healthy # plausibility check
|
||||
uint64 reset_counter # filter reset counter
|
||||
|
||||
@@ -729,7 +729,7 @@ FixedwingPositionINDIControl::Run()
|
||||
// approximate lift force, since implicit equation cannot be solved analytically:
|
||||
// since alpha<<1, we approximate the lift force L = sin(alpha)*Fx - cos(alpha)*Fz
|
||||
// as L = alpha*Fx - Fz
|
||||
float Fx = cosf(_aoa_offset)*body_force(0) - sinf(_aoa_offset)*body_force(2);
|
||||
float Fx = 0.f;//cosf(_aoa_offset)*body_force(0) - sinf(_aoa_offset)*body_force(2);
|
||||
float Fz = -cosf(_aoa_offset)*body_force(2) - sinf(_aoa_offset)*body_force(0);
|
||||
float AoA_approx = (((2.f*Fz)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f) - _C_L0)/_C_L1) /
|
||||
(1 - ((2.f*Fx)/(_rho*_area*(fmaxf(_airspeed*_airspeed,_stall_speed*_stall_speed))+0.001f)/_C_L1));
|
||||
@@ -875,6 +875,14 @@ FixedwingPositionINDIControl::Run()
|
||||
_soaring_controller_wind.position[0] = _pos(0);
|
||||
_soaring_controller_wind.position[1] = _pos(1);
|
||||
_soaring_controller_wind.position[2] = _pos(2);
|
||||
Eulerf e(Quatf(_attitude.q));
|
||||
float bank = e(0);
|
||||
if (fabs(bank)<0.5f) {
|
||||
_soaring_controller_wind.valid = true;
|
||||
}
|
||||
else {
|
||||
_soaring_controller_wind.valid = false;
|
||||
}
|
||||
_soaring_controller_wind_pub.publish(_soaring_controller_wind);
|
||||
|
||||
|
||||
|
||||
@@ -511,8 +511,8 @@ PARAM_DEFINE_INT32(DS_LOITER, 0);
|
||||
* @unit
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_THRUST, 0);
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
-0.000,-3125.685,11089.050,-18942.770,16837.270,-2890.234,-11837.475,13153.936,-141.459,-12990.952,11934.677,2594.466,-16521.457,18740.250,-11010.786,3111.273
|
||||
-99.996,-5095.161,24136.273,-56105.353,80560.079,-70163.336,19749.848,41159.563,-68048.750,40306.618,21058.585,-71404.244,81395.789,-56504.079,24259.930,-5114.204
|
||||
100.000,-0.007,-0.010,-0.082,-0.006,-0.087,0.026,0.106,-0.070,0.008,0.030,-0.377,0.062,-0.129,0.061,-0.003
|
||||
0.000064,3020.233571,-10609.960177,17956.458964,-15735.479961,2399.573434,11421.854705,-12388.936542,-120.944433,12530.869640,-11346.431128,-2643.369342,15999.009519,-18127.094775,10676.696033,-3032.667571
|
||||
-99.999753,-4686.100638,21963.998712,-50566.542735,71908.811380,-61683.065487,15730.546681,39386.062419,-63952.599921,39525.510557,15526.730614,-61505.752731,71804.582585,-50525.803342,21954.858744,-4685.311430
|
||||
100.000000,-0.013254,-0.012083,-0.281296,0.324078,-0.438216,0.063212,0.096122,0.034719,0.060176,0.153558,-0.402818,0.689494,-0.192379,0.050574,-0.011868
|
||||
|
||||
|
@@ -1,3 +0,0 @@
|
||||
-0.000064,-3020.233571,10609.960177,-17956.458964,15735.479961,-2399.573434,-11421.854705,12388.936542,120.944433,-12530.869640,11346.431128,2643.369342,-15999.009519,18127.094775,-10676.696033,3032.667571
|
||||
-100.005984,-4686.100637,21963.998713,-50566.542718,71908.811359,-61683.065460,15730.546677,39386.062413,-63952.599923,39525.510553,15526.730604,-61505.752706,71804.582542,-50525.803330,21954.858741,-4685.311429
|
||||
100.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000,-1875.411,6653.430,-11365.662,10102.362,-1734.140,-7102.485,7892.361,-84.875,-7794.571,7160.806,1556.680,-9912.874,11244.150,-6606.472,1866.764
|
||||
-59.997,-3057.097,14481.764,-33663.212,48336.047,-42098.002,11849.909,24695.738,-40829.250,24183.971,12635.151,-42842.546,48837.473,-33902.447,14555.958,-3068.523
|
||||
100.000,-0.007,-0.010,-0.082,-0.006,-0.087,0.026,0.106,-0.070,0.008,0.030,-0.377,0.062,-0.129,0.061,-0.003
|
||||
0.000038,1812.140143,-6365.976106,10773.875378,-9441.287977,1439.744061,6853.112823,-7433.361925,-72.566660,7518.521784,-6807.858677,-1586.021605,9599.405711,-10876.256865,6406.017620,-1819.600542
|
||||
-59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858
|
||||
100.000000,-0.013254,-0.012083,-0.281296,0.324078,-0.438216,0.063212,0.096122,0.034719,0.060176,0.153558,-0.402818,0.689494,-0.192379,0.050574,-0.011868
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000,-1875.411,6653.430,-11365.662,10102.362,-1734.140,-7102.485,7892.361,-84.875,-7794.571,7160.806,1556.680,-9912.874,11244.150,-6606.472,1866.764
|
||||
-59.997,-3057.097,14481.764,-33663.212,48336.047,-42098.002,11849.909,24695.738,-40829.250,24183.971,12635.151,-42842.546,48837.473,-33902.447,14555.958,-3068.523
|
||||
120.000,-0.008,-0.011,-0.098,-0.007,-0.104,0.031,0.127,-0.084,0.010,0.037,-0.452,0.074,-0.155,0.073,-0.004
|
||||
0.000038,1812.140143,-6365.976106,10773.875378,-9441.287977,1439.744061,6853.112823,-7433.361925,-72.566660,7518.521784,-6807.858677,-1586.021605,9599.405711,-10876.256865,6406.017620,-1819.600542
|
||||
-59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858
|
||||
120.000000,-0.015905,-0.014500,-0.337555,0.388894,-0.525859,0.075855,0.115346,0.041663,0.072211,0.184269,-0.483382,0.827393,-0.230854,0.060689,-0.014241
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000,-3125.685,11089.050,-18942.770,16837.270,-2890.234,-11837.475,13153.936,-141.459,-12990.952,11934.677,2594.466,-16521.457,18740.250,-11010.786,3111.273
|
||||
-99.996,-5095.161,24136.273,-56105.353,80560.079,-70163.336,19749.848,41159.563,-68048.750,40306.618,21058.585,-71404.244,81395.789,-56504.079,24259.930,-5114.204
|
||||
120.000,-0.008,-0.011,-0.098,-0.007,-0.104,0.031,0.127,-0.084,0.010,0.037,-0.452,0.074,-0.155,0.073,-0.004
|
||||
0.000064,3020.233571,-10609.960177,17956.458964,-15735.479961,2399.573434,11421.854705,-12388.936542,-120.944433,12530.869640,-11346.431128,-2643.369342,15999.009519,-18127.094775,10676.696033,-3032.667571
|
||||
-99.999753,-4686.100638,21963.998712,-50566.542735,71908.811380,-61683.065487,15730.546681,39386.062419,-63952.599921,39525.510557,15526.730614,-61505.752731,71804.582585,-50525.803342,21954.858744,-4685.311430
|
||||
120.000000,-0.015905,-0.014500,-0.337555,0.388894,-0.525859,0.075855,0.115346,0.041663,0.072211,0.184269,-0.483382,0.827393,-0.230854,0.060689,-0.014241
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000,-1562.842,5544.525,-9471.385,8418.635,-1445.117,-5918.738,6576.968,-70.730,-6495.476,5967.338,1297.233,-8260.729,9370.125,-5505.393,1555.636
|
||||
-49.998,-2547.580,12068.136,-28052.677,40280.039,-35081.668,9874.924,20579.781,-34024.375,20153.309,10529.293,-35702.122,40697.894,-28252.039,12129.965,-2557.102
|
||||
100.000,781.414,-2772.272,4735.611,-4209.324,722.472,2959.395,-3288.378,35.295,3247.746,-2983.639,-648.993,4130.426,-4685.192,2752.757,-777.821
|
||||
0.000038,1812.140143,-6365.976106,10773.875378,-9441.287977,1439.744061,6853.112823,-7433.361925,-72.566660,7518.521784,-6807.858677,-1586.021605,9599.405711,-10876.256865,6406.017620,-1819.600542
|
||||
-59.999852,-2811.660383,13178.399227,-30339.925641,43145.286828,-37009.839292,9438.328009,23631.637452,-38371.559953,23715.306334,9316.038368,-36903.451639,43082.749551,-30315.482005,13172.915247,-2811.186858
|
||||
99.999981,-906.083325,3182.975970,-5387.218985,4720.968066,-720.310246,-3426.493199,3716.777085,36.318049,-3759.200716,3404.082896,792.607985,-4799.013362,5437.936054,-3202.958236,909.788403
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000,-1562.842,5544.525,-9471.385,8418.635,-1445.117,-5918.738,6576.968,-70.730,-6495.476,5967.338,1297.233,-8260.729,9370.125,-5505.393,1555.636
|
||||
-49.998,-2547.580,12068.136,-28052.677,40280.039,-35081.668,9874.924,20579.781,-34024.375,20153.309,10529.293,-35702.122,40697.894,-28252.039,12129.965,-2557.102
|
||||
100.000,468.846,-1663.367,2841.334,-2525.597,433.449,1775.647,-1972.984,21.149,1948.651,-1790.171,-389.547,2478.280,-2811.167,1651.679,-466.694
|
||||
0.000025,1208.093428,-4243.984071,7182.583586,-6294.191984,959.829374,4568.741882,-4955.574617,-48.377773,5012.347856,-4538.572451,-1057.347737,6399.603807,-7250.837910,4270.678413,-1213.067028
|
||||
-39.999901,-1874.440255,8785.599485,-20226.617094,28763.524552,-24673.226195,6292.218672,15754.424968,-25581.039968,15810.204223,6210.692245,-24602.301093,28721.833034,-20210.321337,8781.943498,-1874.124572
|
||||
99.999987,-604.059968,2121.979952,-3591.573089,3147.420070,-480.352903,-2284.307729,2477.883430,24.223606,-2506.113752,2269.439783,528.271050,-3199.112410,3625.226576,-2135.288633,606.521646
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000,-1250.274,4435.620,-7577.108,6734.908,-1156.094,-4734.990,5261.574,-56.584,-5196.381,4773.871,1037.787,-6608.583,7496.100,-4404.315,1244.509
|
||||
-39.998,-2038.064,9654.509,-22442.141,32224.031,-28065.334,7899.939,16463.825,-27219.500,16122.647,8423.434,-28561.698,32558.315,-22601.632,9703.972,-2045.682
|
||||
100.000,375.075,-1330.695,2273.050,-2020.479,346.741,1420.523,-1578.366,16.905,1558.923,-1432.131,-311.713,1982.637,-2248.959,1321.355,-373.356
|
||||
0.000025,1208.093428,-4243.984071,7182.583586,-6294.191984,959.829374,4568.741882,-4955.574617,-48.377773,5012.347856,-4538.572451,-1057.347737,6399.603807,-7250.837910,4270.678413,-1213.067028
|
||||
-39.999901,-1874.440255,8785.599485,-20226.617094,28763.524552,-24673.226195,6292.218672,15754.424968,-25581.039968,15810.204223,6210.692245,-24602.301093,28721.833034,-20210.321337,8781.943498,-1874.124572
|
||||
99.999992,-362.441283,1273.183138,-2155.056372,1888.581673,-288.387028,-1370.559352,1486.768507,14.548051,-1503.644181,1361.725293,316.801503,-1919.191648,2175.058994,-1281.152950,363.908241
|
||||
|
||||
|
@@ -1,3 +1,3 @@
|
||||
-0.000,-937.705,3326.715,-5682.831,5051.181,-867.070,-3551.243,3946.181,-42.438,-3897.285,3580.403,778.340,-4956.437,5622.075,-3303.236,933.382
|
||||
-29.999,-1528.548,7240.882,-16831.606,24168.024,-21049.001,5924.954,12347.869,-20414.625,12091.985,6317.576,-21421.273,24418.737,-16951.224,7277.979,-1534.261
|
||||
100.000,281.305,-998.024,1704.767,-1515.360,260.034,1065.399,-1183.748,12.661,1169.194,-1074.090,-233.879,1486.993,-1686.751,991.032,-280.017
|
||||
34.908707,2802.143443,-12010.045206,24897.004025,-31652.414085,24086.196989,-4942.033085,-13772.898892,20576.241530,-12668.675896,-4555.658999,21674.320937,-30001.814698,25832.757640,-13941.536701,3638.740016
|
||||
-1.920001,7042.626425,-38404.376575,103774.671218,-174237.276934,184373.717471,-89448.645248,-70134.302861,177608.190474,-136819.046222,-30572.012263,196546.815165,-245393.925168,174924.844259,-73416.451006,14256.585131
|
||||
90.837687,134.620712,-674.566268,1031.769726,671.319554,-5057.527415,8440.006743,-4910.847468,-5701.223344,13715.392464,-8181.508398,-9147.528585,23538.196976,-23336.500927,12535.088204,-3052.472443
|
||||
|
||||
|
@@ -41,6 +41,7 @@ using matrix::Matrix;
|
||||
using matrix::Vector3f;
|
||||
using matrix::Vector;
|
||||
using matrix::wrap_pi;
|
||||
using matrix::geninv;
|
||||
|
||||
|
||||
|
||||
@@ -71,6 +72,11 @@ FixedwingShearEstimator::init()
|
||||
}
|
||||
PX4_INFO("Starting FW_DYN_SOAR_ESTIMATOR");
|
||||
|
||||
// normalization variables
|
||||
_unit_v = 1.f;
|
||||
_unit_h = 1.f;
|
||||
_unit_a = 1.f;
|
||||
|
||||
parameters_update();
|
||||
|
||||
// init horizontal wind field
|
||||
@@ -95,9 +101,12 @@ FixedwingShearEstimator::init()
|
||||
_P_prior_vertical = _Q_vertical;
|
||||
_P_posterior_vertical = _Q_vertical;
|
||||
|
||||
|
||||
//
|
||||
_X_prior_horizontal(4) = _init_height;
|
||||
_X_posterior_horizontal(4) = _init_height;
|
||||
_X_prior_horizontal(5) = 0.5f/_unit_a;
|
||||
_X_posterior_horizontal(5) = 0.5f/_unit_a;
|
||||
|
||||
// init time
|
||||
_last_run = hrt_absolute_time();
|
||||
@@ -114,15 +123,17 @@ FixedwingShearEstimator::parameters_update()
|
||||
updateParams();
|
||||
|
||||
// update params...
|
||||
_Q_horizontal(0,0) = powf(_param_sigma_q_vel.get(),2);
|
||||
_Q_horizontal(1,1) = powf(_param_sigma_q_vel.get(),2);
|
||||
_Q_horizontal(2,2) = powf(_param_sigma_q_vel.get(),2);
|
||||
_Q_horizontal(3,3) = powf(_param_sigma_q_vel.get(),2);
|
||||
_Q_horizontal(4,4) = powf(_param_sigma_q_h.get(),2);
|
||||
_Q_horizontal(5,5) = powf(_param_sigma_q_a.get(),2);
|
||||
_Q_horizontal.setZero();
|
||||
_Q_horizontal(0,0) = powf(_param_sigma_q_vel.get()/_unit_v,2);
|
||||
_Q_horizontal(1,1) = powf(_param_sigma_q_vel.get()/_unit_v,2);
|
||||
_Q_horizontal(2,2) = powf(_param_sigma_q_vel.get()/_unit_v,2);
|
||||
_Q_horizontal(3,3) = powf(_param_sigma_q_vel.get()/_unit_v,2);
|
||||
_Q_horizontal(4,4) = powf(_param_sigma_q_h.get()/_unit_h,2);
|
||||
_Q_horizontal(5,5) = powf(_param_sigma_q_a.get()/_unit_a,2);
|
||||
|
||||
_R_horizontal(0,0) = powf(_param_sigma_r_vel.get(),2);
|
||||
_R_horizontal(1,1) = powf(_param_sigma_r_vel.get(),2);
|
||||
_R_horizontal.setZero();
|
||||
_R_horizontal(0,0) = powf(_param_sigma_r_vel.get()/_unit_v,2);
|
||||
_R_horizontal(1,1) = powf(_param_sigma_r_vel.get()/_unit_v,2);
|
||||
|
||||
for (int i=0;i<(int)_dim_vertical;i++){
|
||||
_Q_vertical(i,i) = powf(_param_sigma_q_vel.get(),2);
|
||||
@@ -130,7 +141,7 @@ FixedwingShearEstimator::parameters_update()
|
||||
|
||||
_R_vertical(0,0) = powf(_param_sigma_r_vel.get(),2);
|
||||
|
||||
_init_height = _param_init_h.get();
|
||||
_init_height = _param_init_h.get()/_unit_h;
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
@@ -144,17 +155,17 @@ FixedwingShearEstimator::reset_filter()
|
||||
for (uint i=0;i<6;i++){
|
||||
_X_prior_horizontal(i) = 0.0f;
|
||||
_X_posterior_horizontal(i) = 0.0f;
|
||||
_P_prior_horizontal(i,i) = 1.0f;
|
||||
_P_posterior_horizontal(i,i) = 1.0f;
|
||||
}
|
||||
_P_prior_horizontal = _Q_horizontal;
|
||||
_P_posterior_horizontal = _Q_horizontal;
|
||||
|
||||
// reset vertical wind state
|
||||
for (uint i=0;i<_dim_vertical;i++){
|
||||
_X_prior_vertical(i) = 0.0f;
|
||||
_X_posterior_vertical(i) = 0.0f;
|
||||
_P_prior_vertical(i,i) = 1.0f;
|
||||
_P_posterior_vertical(i,i) = 1.0f;
|
||||
}
|
||||
_P_prior_vertical = _Q_vertical;
|
||||
_P_posterior_vertical = _Q_vertical;
|
||||
|
||||
// set height to enable convergence
|
||||
_X_prior_horizontal(4) = _init_height;
|
||||
@@ -190,16 +201,20 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
|
||||
// first fill the horizontal observation matrix:
|
||||
float vx = _X_prior_horizontal(0);
|
||||
float vy = _X_prior_horizontal(1);
|
||||
//float bx = _X_prior_horizontal(1);
|
||||
//float by = _X_prior_horizontal(3);
|
||||
float h = _X_prior_horizontal(4);
|
||||
float a = _X_prior_horizontal(5);
|
||||
|
||||
_H_horizontal.setZero();
|
||||
_H_horizontal(0,0) = 1.f/(1.f + expf(-(height-h)*a));
|
||||
_H_horizontal(1,1) = 1.f/(1.f + expf(-(height-h)*a));
|
||||
_H_horizontal(0,2) = 1.f;
|
||||
_H_horizontal(1,3) = 1.f;
|
||||
_H_horizontal(0,4) = -((a*vx*expf(-(height-h)*a)))/powf(1.f + expf(-(height-h)*a),2);
|
||||
_H_horizontal(1,4) = -((a*vy*expf(-(height-h)*a)))/powf(1.f + expf(-(height-h)*a),2);
|
||||
_H_horizontal(0,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf(1.f + expf(-(height-h)*a),2);
|
||||
_H_horizontal(1,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf(1.f + expf(-(height-h)*a),2);
|
||||
_H_horizontal(0,4) = -((a*vx*expf(-(height-h)*a)))/powf((1.f + expf(-(height-h)*a)),2.f);
|
||||
_H_horizontal(1,4) = -((a*vy*expf(-(height-h)*a)))/powf((1.f + expf(-(height-h)*a)),2.f);
|
||||
_H_horizontal(0,5) = -((vx*(h-height))*expf(-(height-h)*a))/powf((1.f + expf(-(height-h)*a)),2.f);
|
||||
_H_horizontal(1,5) = -((vy*(h-height))*expf(-(height-h)*a))/powf((1.f + expf(-(height-h)*a)),2.f);
|
||||
|
||||
// then fill the vertical observation matrix
|
||||
for (uint i=0;i<_dim_vertical;i++){
|
||||
@@ -208,16 +223,10 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
|
||||
|
||||
// compute Kalman gain matrix for horizontal wind states
|
||||
Matrix<float, 6, 2> tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T();
|
||||
Matrix<float, 2, 2> tmp2_horizontal = _H_horizontal*_P_prior_horizontal*_H_horizontal.T() + _R_horizontal;
|
||||
Matrix<float, 2, 2> inv_horizontal;
|
||||
inv_horizontal(0,0) = tmp2_horizontal(1,1);
|
||||
inv_horizontal(0,1) = -tmp2_horizontal(0,1);
|
||||
inv_horizontal(1,1) = tmp2_horizontal(0,0);
|
||||
inv_horizontal(1,0) = -tmp2_horizontal(1,0);
|
||||
Matrix<float, 2, 2> tmp2_horizontal = _H_horizontal*(_P_prior_horizontal*_H_horizontal.T()) + _R_horizontal;
|
||||
float determinant_horizontal = tmp2_horizontal(0,0)*tmp2_horizontal(1,1) - tmp2_horizontal(1,0)*tmp2_horizontal(0,1);
|
||||
if (fabs(determinant_horizontal)>0.000001f){
|
||||
inv_horizontal /= determinant_horizontal;
|
||||
_K_horizontal = tmp1_horizontal*inv_horizontal;
|
||||
_K_horizontal = tmp1_horizontal*geninv(tmp2_horizontal);
|
||||
}
|
||||
else{
|
||||
PX4_WARN("singular horizontal matrix, resetting filter");
|
||||
@@ -226,7 +235,7 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
|
||||
|
||||
// then fill the vertical observation matrix
|
||||
for (uint i=0;i<_dim_vertical;i++){
|
||||
_H_vertical(0,i) = powf(height,i);
|
||||
_H_vertical(0,i) = powf(height-h,i);
|
||||
}
|
||||
|
||||
// compute Kalman gain matrix for vertical wind states
|
||||
@@ -248,12 +257,18 @@ FixedwingShearEstimator::perform_posterior_update(float height, Vector3f wind)
|
||||
}
|
||||
else {
|
||||
// perform horizontal update
|
||||
Vector<float, 2> z_expected_horizontal = (Vector<float, 2>) (_H_horizontal*_X_prior_horizontal);
|
||||
Vector<float, 2> z_expected_horizontal;
|
||||
Vector<float, 2> wind_horizontal;
|
||||
Matrix<float, 6, 6> identity_1;
|
||||
wind_horizontal(0) = wind(0);
|
||||
wind_horizontal(1) = wind(1);
|
||||
z_expected_horizontal(0) = _X_prior_horizontal(0)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(2);
|
||||
z_expected_horizontal(1) = _X_prior_horizontal(1)/(1.f+expf(-_X_prior_horizontal(5)*(_current_height-_X_prior_horizontal(4)))) + _X_prior_horizontal(3);
|
||||
z_expected_horizontal.print();
|
||||
wind_horizontal(0) = _current_wind(0);
|
||||
wind_horizontal(1) = _current_wind(1);
|
||||
identity_1.setIdentity();
|
||||
//z_expected_horizontal.print();
|
||||
//wind_horizontal.print();
|
||||
//(_K_horizontal*(wind_horizontal - z_expected_horizontal)).print();
|
||||
_X_posterior_horizontal = _X_prior_horizontal + _K_horizontal*(wind_horizontal - z_expected_horizontal);
|
||||
_P_posterior_horizontal = (identity_1 - _K_horizontal*_H_horizontal)*_P_prior_horizontal;
|
||||
|
||||
@@ -290,7 +305,8 @@ FixedwingShearEstimator::Run()
|
||||
perf_begin(_loop_perf);
|
||||
|
||||
// only run controller if wind info changed
|
||||
if (_soaring_controller_wind_sub.update(&_soaring_controller_wind))
|
||||
soaring_controller_wind_s soaring_controller_wind;
|
||||
if (_soaring_controller_wind_sub.update(&soaring_controller_wind))
|
||||
{
|
||||
// only update parameters if they changed
|
||||
bool params_updated = _parameter_update_sub.updated();
|
||||
@@ -306,30 +322,46 @@ FixedwingShearEstimator::Run()
|
||||
parameters_update();
|
||||
}
|
||||
// get current measurement
|
||||
_current_wind = Vector3f(_soaring_controller_wind.wind_estimate_filtered);
|
||||
_current_height = Vector3f(_soaring_controller_wind.position)(2);
|
||||
_current_wind = Vector3f(soaring_controller_wind.wind_estimate_filtered)/_unit_v;
|
||||
_current_height = Vector3f(soaring_controller_wind.position)(2)/_unit_h;
|
||||
//_current_wind(0) = 0.f;
|
||||
//_current_wind(1) = -(10.f/_unit_v)/(1+expf(-1.f/_unit_a*(_current_height- 110.f/_unit_h)));
|
||||
//_current_wind(2) = 0.f;
|
||||
|
||||
// prior update
|
||||
perform_prior_update();
|
||||
if (true) {
|
||||
// prior update
|
||||
perform_prior_update();
|
||||
|
||||
// posterior update
|
||||
perform_posterior_update(_current_height, _current_wind);
|
||||
// posterior update
|
||||
perform_posterior_update(_current_height, _current_wind);
|
||||
|
||||
// check if filter diverges
|
||||
// maybe reset filters...
|
||||
//reset_filter();
|
||||
|
||||
|
||||
// check if filter diverges
|
||||
// maybe reset filters...
|
||||
if (sqrtf(_P_posterior_horizontal(4,4))>=10.f) {
|
||||
PX4_WARN("large height uncertainty, resetting filter");
|
||||
reset_filter();
|
||||
}
|
||||
}
|
||||
|
||||
// publish shear params
|
||||
// ========================================
|
||||
// publish controller position in ENU frame
|
||||
// ========================================
|
||||
_soaring_estimator_shear.timestamp = hrt_absolute_time();
|
||||
_soaring_estimator_shear.vx = _X_posterior_horizontal(0);
|
||||
_soaring_estimator_shear.vy = _X_posterior_horizontal(1);
|
||||
_soaring_estimator_shear.bx = _X_posterior_horizontal(2);
|
||||
_soaring_estimator_shear.bx = _X_posterior_horizontal(3);
|
||||
_soaring_estimator_shear.h = _X_posterior_horizontal(4);
|
||||
_soaring_estimator_shear.a = _X_posterior_horizontal(5);
|
||||
_soaring_estimator_shear.vx = _X_posterior_horizontal(0)*_unit_v;
|
||||
_soaring_estimator_shear.vy = _X_posterior_horizontal(1)*_unit_v;
|
||||
_soaring_estimator_shear.bx = _X_posterior_horizontal(2)*_unit_v;
|
||||
_soaring_estimator_shear.by = _X_posterior_horizontal(3)*_unit_v;
|
||||
_soaring_estimator_shear.h = _X_posterior_horizontal(4)*_unit_h;
|
||||
_soaring_estimator_shear.a = _X_posterior_horizontal(5)*_unit_a;
|
||||
_soaring_estimator_shear.sigma_vx = sqrtf(_P_posterior_horizontal(0,0))*_unit_v;
|
||||
_soaring_estimator_shear.sigma_vy = sqrtf(_P_posterior_horizontal(1,1))*_unit_v;
|
||||
_soaring_estimator_shear.sigma_bx = sqrtf(_P_posterior_horizontal(2,2))*_unit_v;
|
||||
_soaring_estimator_shear.sigma_by = sqrtf(_P_posterior_horizontal(3,3))*_unit_v;
|
||||
_soaring_estimator_shear.sigma_h = sqrtf(_P_posterior_horizontal(4,4))*_unit_h;
|
||||
_soaring_estimator_shear.sigma_a = sqrtf(_P_posterior_horizontal(5,5))*_unit_a;
|
||||
_soaring_estimator_shear.reset_counter = _reset_counter;
|
||||
_soaring_estimator_shear_pub.publish(_soaring_estimator_shear);
|
||||
}
|
||||
|
||||
@@ -130,6 +130,10 @@ private:
|
||||
Vector3f _current_wind = {};
|
||||
float _current_height = {};
|
||||
|
||||
float _unit_v;
|
||||
float _unit_h;
|
||||
float _unit_a;
|
||||
|
||||
// helper variables
|
||||
float _init_height = {};
|
||||
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.1f);
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.01f);
|
||||
|
||||
/**
|
||||
* Standard deviation of vertical shear position
|
||||
@@ -36,7 +36,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_V, 0.1f);
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f);
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 0.1f);
|
||||
|
||||
/**
|
||||
* Standard deviation of velicity state in shear model
|
||||
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_H, 1.f);
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.03f);
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.003f);
|
||||
|
||||
/**
|
||||
* Standard deviation of velicity measurement (wind)
|
||||
@@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(DS_SIGMA_Q_A, 0.03f);
|
||||
* @increment 0.000001
|
||||
* @group FW DYN SOAR Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 1.f);
|
||||
PARAM_DEFINE_FLOAT(DS_SIGMA_R_V, 4.f);
|
||||
|
||||
/**
|
||||
* Initial guess of the vertical shear position
|
||||
|
||||
Reference in New Issue
Block a user