diff --git a/msg/soaring_controller_wind.msg b/msg/soaring_controller_wind.msg index a3efee6224..60bc5cdf63 100644 --- a/msg/soaring_controller_wind.msg +++ b/msg/soaring_controller_wind.msg @@ -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 diff --git a/msg/soaring_estimator_shear.msg b/msg/soaring_estimator_shear.msg index 103d35c3f4..7ddbe53faf 100644 --- a/msg/soaring_estimator_shear.msg +++ b/msg/soaring_estimator_shear.msg @@ -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 diff --git a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp index 01a5ccbaef..7366ceeafe 100644 --- a/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp +++ b/src/modules/fw_dyn_soar_control/FixedwingPositionINDIControl.cpp @@ -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); diff --git a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c index f9a19dd97a..4869fc9435 100644 --- a/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c +++ b/src/modules/fw_dyn_soar_control/fw_dyn_soar_control_params.c @@ -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); diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv index 8af63eaa40..240d82f423 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory0.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory0_sigma_1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory0_sigma_1.csv deleted file mode 100644 index 7df166f0e7..0000000000 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory0_sigma_1.csv +++ /dev/null @@ -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 \ No newline at end of file diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv index 8eab16b5fa..52f729827d 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory1.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv index 450343e8fd..acc5304325 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory2.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv index 94cb374efe..f9838f54b4 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory3.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv index 1bd59afe96..0347087ed5 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory4.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv index 266a5bceb0..7b8334eab1 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory5.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv index ac85ee87af..bc32e9dd02 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory6.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv index 7bed3894d0..506a3cf994 100644 --- a/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv +++ b/src/modules/fw_dyn_soar_control/trajectories/trajectory7.csv @@ -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 diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp index 80236cb6d5..6ff6a68d81 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.cpp @@ -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 tmp1_horizontal = _P_prior_horizontal*_H_horizontal.T(); - Matrix tmp2_horizontal = _H_horizontal*_P_prior_horizontal*_H_horizontal.T() + _R_horizontal; - Matrix 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 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 z_expected_horizontal = (Vector) (_H_horizontal*_X_prior_horizontal); + Vector z_expected_horizontal; Vector wind_horizontal; Matrix 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); } diff --git a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp index 28bfa3969a..a2b830803d 100644 --- a/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp +++ b/src/modules/fw_dyn_soar_estimator/FixedwingShearEstimator.hpp @@ -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 = {}; diff --git a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c index 4f55421df1..58fe7a52eb 100644 --- a/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c +++ b/src/modules/fw_dyn_soar_estimator/fw_dyn_soar_estimator_params.c @@ -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