Queries

[samuelctabor]

Hi there,

I'm working on a simplified version to act as a starting point to getting the POMDP into the main codebase. For the following settings:

• POMDP_NORM: Normalises the covariance trace (= 1 in param file)
• POMDP_EXT: Extends action duration in lift-exploiting mode (= 3 in param file)
• POMDP_PLAN: Enable deterministic trajectory planning mode (= 0 in param file)

Do you find that a particular configuration gives the best performance?

I'm also wondering about the relationship between N, HORI and K (samples per action trajectory, planning horizon duration and samples per second) - it seems like only two should be needed?

Lastly, the scheduling - there's several entries in the scheduler table calling soaring_policy_computation at 50Hz. This is unlikely to be acceptable - is there another way to achieve this? Would calling it at a higher rate have the same effect?

Cheers!

[samuelctabor]

PR opened against the official repo:

https://github.com/ArduPilot/ardupilot/pull/9042/files

Any testing would be great! For now I removed most of the features not directly POMDP related - both soaring types now use LOITER. It seems to run OK in SITL but I haven't tried it in thermals yet.

Cheers,
Sam

[iainguilliard]

Hi Sam,

Running a POMDP solver scheduled as a real time process within an autopilot was more of a technology demonstrator of our research. But like any real world RT solution, it is statically configured for a particular fork of Arduplane and a particular vehicle. Any change to either requires careful timing tests and a reconfiguration and so it is probably not a good fit for porting back for main stream use as it is. A better approach would be to use a low priority background process that soaks up the CPU idle time.

The solver is relatively insensitive to run time jitter and can be reorganised to converge on the optimum solution over time: Currently it iterates sequentially over the actions, evaluating a fixed number (POMDP_N) of state space samples for each. But if you swap these loops so that it draws a sample and evaluates the reward function over the actions, and then draws another sample, and so on until it hits the deadline, then the current solution will be the most optimal one found within time available, and the user can trade between solve time and update rate to tune performance. And the performance should degrade gracefully with variations in CPU load. In fact, I have implemented it this way in a multithreaded python simulation and it works very well.

We always intended to run the solver off board (the original reason for the soaring MAVLink messages), on a more powerful daughter board such as a Raspberry Pi, and the fact that worked at all on Pixhawk was something of a surprise!

But if limited to such a small platform as the Pixhawk, and with a lot of CPU contention, ultimately the better approach would be to use a table driven solver. This re-casts the resource problem as a spacial one rather than a temporal one, and given that the user can just add a suitable sized SD card as needed, and we don't know what future ArduPlane features will do to the CPU load, this would be the way to go if you really want to port it back.

Perhaps as a first step you could just port the controller back as a model predictive controller. This is essentially what the POMDP_PLAN parameter does. It stops solving the POMDP and just evaluates the performance of each action given the current EKF estimate of the thermal, choosing the best one. This should run easily at 5Hz within the time slice currently allocated to the soaring controller, and you can ditch the 50Hz solver tasks.

In the mean time we can work here on a threaded version of the controller for Frigatebird. This will have other benefits: The solver code can then be run with out any changes either in a local background thread, or off board, and, it can be implemented as more generic library or module in its own right, such that it could be reused for other applications (There was quite a lot of interest at RSS from people with other control tasks on mind).

[iainguilliard]

And the other part of your question, with some more details on the POMDP solver parameters:

POMDP_NORM: Normalizes the covariance trace, to make the value less sensitive to variations in thermal size and shape. More of a hangover from development, so could be removed and solver always run in normalized mode.

POMDP_PTH: related to POMDP_NORM, the threshold of the normed tr(P) to switch the reward function between explore and exploitation.

POMDP_N, POMDP_K and POMDP_N_ACT, are the discretization rates for the POMDP solver of the continuous space represented by the thermal estimate and its uncertainty, and the simulation of the sailplane.

POMDP_N: the number of belief space samples used to evaluate the reward function for an action.

POMDP_K: the rate as which the reward function is evaluated along each action, i.e. the discretization along each action. Ideally this should at least match the rate at which the EKF is run, which is currently 5Hz. Since this is fixed in the code for the soaring controller, POMDP_K could be removed as a parameter. However, the POMDP could be further approximated by stepping more coarsely along the action perhaps allowing a longer horizon to be evaluated more quickly. We never tested this beyond 5Hz so something still to try, and the EKF Q and R matrixes would have to be adjusted in the solver accordingly.

POMDP_HORI: The length of the actions in seconds, i.e. the planning horizon of the solver. Again, needs to be traded off between the number of samples and the number of actions, or the rate of reward function evaluation. Since there is no action branching, i.e. each action is limited to a single banked turn, the performance is sensitive to the choice of planning horizon

POMDP_EXT: extension factor of the POMDP_HORI when in exploitation mode. The idea here is that when the uncertainty of the thermal estimate is high (i.e. during acquisition), then focus the solver on quickly reducing the uncertainty, and replan rapidly as any plan will quickly become stale as the estimate evolves. Once the estimate is stable, focus on soaring and with longer planning horizons.
Also extends the solver step time, POMDP_STEP, by the same amount.

A better approach might be to continuously scale and blend between these extremes, and with a threaded, converging solver this would be possible, but with the RT solver and its statically configured timing that wasn't practical. In any case, since the planning is done in steps that are typically longer than the rate at which the estimate evolves, it still tends to switch between extremes.

POMDP_STEP: the time it takes the solver to complete in explore mode (horizon extension factor = 1), given the product of the above parameters: N samples x K evaluation points x HORI seconds x N_ACTions. Note all the actions currently being evaluated in the solver will start with the current action being executed by the sailplane for the first POMDP_STEP x POMDP_EXT seconds.

POMDP_N_ACT, POMDP_ROLL1, POMDP_ROLL2: the number and range of actions where each action is a commanded bank angle roll to and hold until the the next planning update. Why two parameters for the range? Well actually you can just run the solver over actions in one direction by specifying a positive value for both, and the controller will choose which direction, left or right, based on the which side of the flight path the thermal is estimated to be when first engaged. But its nicer from an AI point of view to let the solver choose from the actions in both directions.

Do you find that a particular configuration gives the best performance?

All of the above parameter values were tuned through actual flight testing to give the best performance for the Radian Pro and Pixhawk, and these values are in the file Frigatebird/DATA/RSS-2018_Radian_Pro_parameters.param.

I'm also wondering about the relationship between N, HORI and K (samples per action trajectory, planning horizon duration and samples per second) - it seems like only two should be needed?

All are needed, but K could be hard coded to match the EKF rate.

POMDP_PLAN: Effectively run the controller as a Model Predictive Controller, but just use the mean as a single sample (i.e. the state vector of the EKF) to plan rather than solve a POMDP by sampling over the belief space.

POMDP_LOOP: Critical timing parameter for the solver task. The number of samples to evaluate per time slice. Found through timing tests on actual hardware and checked in test flight logs for overruns.

[iainguilliard]

both soaring types now use LOITER. It seems to run OK in SITL but I haven't tried it in thermals yet.

Nice idea to use LOITER mode for POMDSoar. Even better would be to add a dedicated SOARING mode to Arduplane. To that end, I've implemented a simple bank angle controller that orbits around a point just like LOITER mode but performs much better, i.e. can circle much tighter, and can also be fed an energy acceleration signal (E_dot_dot) to improve centering as per Reichmann's method and as used by Allen and Anderson in their autonomous soaring work. This would benefit the original ArduSoar controller.

[samuelctabor]

Hi Iain,

Thanks for the detailed responses.

Perhaps as a first step you could just port the controller back as a model predictive controller. This is essentially what the POMDP_PLAN parameter does. It stops solving the POMDP and just evaluates the performance of each action given the current EKF estimate of the thermal, choosing the best one. This should run easily at 5Hz within the time slice currently allocated to the soaring controller, and you can ditch the 50Hz solver tasks.

Sounds good, I will update the PR to use only the planning mode for now and remove the scheduled solver updates.

Out of interest, did you consider taking account of the uncertainty in thermal position in EXPLOIT mode by using the expectation of lift as the cost function, rather than by sampling N thermals? In EXPLORE mode perhaps it's possible to compute the expectation of covariance reduction too.

Thanks for the parameter descriptions, I think I understand better now. I'll make some simplifications to reduce the number of parameters as you suggest.

The bank angle controller sounds very useful, I'd be keen to try it out.

For further development would you be interested in working on the version I've created based on master? If you'd rather continue on this version I will create a PR to back-port the changes to the POMPD method so that we can stay in sync.

[samuelctabor]

I've done some initial comparisons with my version here https://github.com/samuelctabor/ardupilot/tree/Soar-POMDP-thermals

For this I have removed the 50Hz solver tasks and set POMDP_PLAN=1. I have configured it to regenerate all the action trajectories and evaluate all actions on each update of the soaring controller. I've set up a SITL case so that each controller configuration starts thermalling in an identical state. I've found that the time horizon needs to be reasonable long - I'm using 10s here. There are three cases shown - POMDP off (normal L1 controller), POMDP on1 (N_ACTIONS=5) and POMDP on2 (N_ACTIONS=10). The first one successfully centres the thermal. The second loses it, and the third centres it a bit slower than the first.

The POMDP parameters I'm using aren't fully tuned and I'm going to keep playing with them. From what I've seen so far I think that the method of selecting a constant bank angle for the full planning horizon makes centreing the thermal quite difficult. Allowing bank angle to change would be a lot better but would obviously increase the combinations massively.

Flight path:

Error in thermal position estimate:

Roll angle:

Distance to thermal centre:

Altitude:

[iainguilliard]

Hey Sam, nice work! what simulator are you using?

Are you still using explore mode? That might not work so well without the help of the POMDP solver so try setting SOAR_POMDP_PTH to 10000 to disable it. Do you have SOAR_POMDP_EXT set > 1? i.e. is your 10sec horizon actually 10 x SOAR_POMDP_EXT sec? It looks like you are simulating large thermals (R > 50m). Is INITIAL_THERMAL_RADIUS in AP_Soaring.h still set to 30m? Also, don't forget to adjust SOAR_POMDP_STEP to match the time the planner takes to run and update the command, so that the actions under evaluation match the action currently being executed for the first SOAR_POMDP_STEP seconds. Otherwise your predictions wont match the actual flight path (try overlaying them on your SITL plots as a check)

POMDSoar works best with thermals that are small relative to the turning circle of the sailplane, and the supplied parameter file was tuned for a Radian Pro at 9m/s for thermals around 30m and smaller (note we changed INITIAL_THERMAL_RADIUS to 30m from your default of 100m in ArduSoar). If you think about it, there are a limited set of useful actions that a sailplane can do with a small, close in thermal - switch turning direction or fine tune the bank angle of the current turn, and we exploited this to get a myopic POMDP running on a Pixhawk.

But as you point out, using the planner with a longer horizons exasperates the effects of the discretization and situations where none of the actions are particularly close to the optimal flight path become more probable. The best solution is branching, and I would think there is enough time left in the Soaring slice to do at least a level or two of branching. You can also reduce the number of actions at each branch point to improve solve time, i.e., try just having hold bank, increase and decrease at later branch points - you are still enriching the policies the planner has to choose from. Also, remember you can run POMDSoar across multiple Soaring slices if you need more time - the solver keeps track of its context.

Another approach that should be quick to implement, is to simulate branching with macro actions. Have more actions with longer horizons but change the bank angle at points along each action. You might have, say, a subset of actions that explores closer in and a set that first fly further away then turn more tightly to explore further out.

Another really simple thing you can try with long horizons is to add a discount factor. That is to weight the reward function along each action (or path of actions with branching), such that that the earlier parts contribute more than than later parts. Typically this is done with a weight of gamma^t where t is the time along the action and gamma in [0,1] is a discount factor:

[samuelctabor]

Hi Iain,

It's the built-in SITL model, with some tweaks to the coefficients to make then more Radian-like (reduced mass and drag).

I am using the EXPLORE mode, but I have seen that it generally goes to EXPLOIT quickly with my setup. POMD_EXT is zero so it really is 10s I believe.

In those sims the thermal radius is 80m, and the initial estimate is 30m. So it has quite a bit of learning to do.

I have STEP set to 1.0. I am not 100% sure what this one does - does it mean that the new action won't be applied until 1 second into the trajectory? I haven't tried overlaying the trajectories.

With regards to timing, I have set ACTION_GENERATION_STEPS_PER_LOOP to 1000 and POMD_LOOP to 100000 and I call _solver.update Nactions+1 times on each call to POMDSoarAlgorithm::update_thermalling. That way a new trajectory is calculated for each action and all the actions are evaluated on each update. I am aiming for the best case for the POMDP solver here.

I see that just now _sign is calculated in init_thermalling. There may be some benefit to updating it dynamically based on the estimated thermal position. The default controller certainly switches direction during initial centreing.

For now here's a rerun with 30m radius thermal. The POMDP-off is still doing better, although the N_ACTIONS=10 is matching it's climb rate due to a higher average bank angle.

Note also that the strength (4m/s) and radius (30.0m) estimates are better with POMDP on, this is because the circling radius is less consistent so these states are better distinguished; there's no way to tell them apart when circling radius is constant.

I like the idea of the branching controller, that should make it possible to centre the thermal better. Have you looked at finding the truly optimal trajectory via Euler-Lagrange or dynamic programming? This would provide a gold standard trajectory for comparison.

Cheers,
Sam

[iainguilliard]

10 sec sounds a bit long to me for the initial thermal centring. Here, we found 4 seconds worked well, and then extending it to 12 sec once the estimate was stable, hence the explore/exploit architecture and extend parameter.

It is important that the solver's flight path predictions match reasonably well with the actual flight path for the same control input. You may need to retune the solvers simulation parameters: I_MOMENT, K_AILERON, K_ROLLDAMP and ROLL_CLP to match the dynamics of your simulated plane.

Again, I suggest debugging by comparing or overlaying the actions with the resulting flight path. The first second of the chosen action is logged in the DF message PDBG:

https://github.com/Microsoft/Frigatebird/blob/773a259e93fd1305bcc8c678c1c7ff527b071f0d/libraries/AP_Soaring/POMDSoar.cpp#L418

And all the actions are sent out over mavlink:

https://github.com/Microsoft/Frigatebird/blob/773a259e93fd1305bcc8c678c1c7ff527b071f0d/libraries/AP_Soaring/POMDSoar.cpp#L212

But it might be just as easy to to add some code to dump them to a file.

You need to set STEP to match the time it takes the solver to run which is determined by the ACTION_GENERATION_STEPS_PER_LOOP and POMD_LOOP parameters (0.6 - 1.8 sec in our experiments). This is because the first part of the every action up to STEP seconds needs to be the flight path resulting from the command that is currently executing and was selected in the previous step of the solver. But it sounds like you are running the solver very fast, so this will be effectively zero. With it set to 1 sec, the prediction wont match the actual flight path and performance will degrade.

[samuelctabor]

Hi Iain,

Ok great, thanks for the explanation. I've done some comparisons of the planned and achieved roll angles as a first step to comparing the flight paths and fixed the _STEP issue. I've reduced the re-planning rate to 1Hz to exaggerate the effect.

As you can see there is a mismatch between the dynamics at present, the simulated aircraft is more oscillatory so I will bring these into alignment.

Nearly IROS time - exciting to see two other papers on autonomous soaring!

Sam