Performance improvement tips

[akash901]

Hello @TomLKoller , First of all Thank you for creating such wonderful research open source. I am going through code and your published paper as well and I had few queries.

1. I want to utilize BoxPlus IMM filter for future state estimation of aircraft/ UAV say after 2/3 secs. Is it possible with current implemetation?
2. I ran SmoothVectorFlightExample code and got RMSE of 0.157 for RTS-Smoother IMM filter. Can you give me tips to improve the RMSE further for increasing accuracy?

[TomLKoller]

Hi @akash901, Thank you for your kind words. I hope i can help you with your queries:

1. From what i understand you want to run the filter as usual but also have an estimate of the future state. In principle, you can estimate the future by calling .predictWithNonAdditiveNoise(deltaT) with using the desired deltaT and calling combination() to get the combinated result. The problem with that is, that the inner filters only save one state so that predicting the future would overwrite your "present" estimation. You could overcome this by implementing another predictFuture method in the IMM which saves the state and covariance of the inner filter (.mu and .sigma), combines them and restores the old values.

Another possibility would be to put the future in the state i.e. your state would be x=(present state, future state). Then you apply the dynamic models on both states but the measurements only on the present state. This is basically the reverse of implementing a fixed lag smoother. I havent tried this so i cant tell if it works. If you try it i would be interested to see the results.

2. I can only give you limited advise on that. In general, the estimate RMSE depends on two things. The noise present in the sensor readings and the accuracy of your models. Since you cant improve sensor accuracy (technically, since it is a simulation, you can by adapting "rad_sigma" line 109) you have to tweak your models. You can tweak the noises of the models and the transition probabilities. You could also adapt the models itself if your UAV has more flight modes.

To tweak the transition probabilities you can plot the model probabilities by calling adekf::viz::plotVector(rts_imm.getModelProbabilities(),"Model Probabilities",path.path.size(),"sc"); after update(...) You have to place the initGuis() command from the end to the beginning of the plot function to see the plot.

Another possibility would be to implement the method in: "IMM Estimators with Unbiased Mixing for Tracking Targets Performing Coordinated Turns" John D. Glass et. al. 2013

The constant turn model and the straight model both use the position and velocity but only the constant turn model uses the angular rate. This introduces a bias on the angular rate induced by the mixing of the states. The idea in the paper is to only mix the states that are used for each model. If you look into the BPIMM.h you will see that in weightedStateSum and weightedCovarianceSum lines are commented out that contain a try of me to implement this feature. The implementation didnt made it in the final publication. It is incomplete at the moment. If you implement this please create a pull request so i can make it available.

[akash901]

Thank you @TomLKoller for such nice explanation and helping me out for finding future states. I implemented your first suggestion for saving the state and covariance of the inner filter (.mu and .sigma), combining them and restoring the old values. This also worked well. I am attaching future state estimation for 2 sec in future with RTS-IMM without smoothing but results are not ideal yet. I have follow-up questions :

1. Can we apply RTS-Smoother on future predicted States? I am facing trouble in doing so with current code. I implemented top level API for accessing smoother for future states and also saved future states in seperate aligned_vector's but getting below error

   Indefinit Count: 0
   Improved Count: 3199
   Future MUS : 3201 all control size : 3200
   SmoothVectorFlightExample: /home/akash/Work//Boxplus-IMM/BPIMM.h:211: State adekf::BPIMM<Filter, State, DYNAMIC_NOISE_TYPE, Models>::weightedStateSum(const Eigen::Matrix<double, -1, 1>&, const State&, const GetState&) [with GetState = adekf::BPRTSIMM<Filter, State, DYNAMIC_NOISE_TYPE, Models>::smoothFutureIntervalWithNonAdditiveNoise(std::size_t, int, const std::vector<std::tuple<_Args1 ...> >&) [with Controls = {double}; Filter = adekf::RTS_Smoother; State = Vector_State<double>; DYNAMIC_NOISE_TYPE = Eigen::Matrix<double, 3, 3>; Models = {main(int, char**)::<lambda(auto:79&, auto:80, double)>, constant_turn_model}; std::size_t = long unsigned int]::<lambda(auto:77&)>; Filter = adekf::RTS_Smoother; State = Vector_State<double>; DYNAMIC_NOISE_TYPE = Eigen::Matrix<double, 3, 3>; Models = {main(int, char**)::<lambda(auto:79&, auto:80, double)>, constant_turn_model}]: Assertion `abs(probabilities.sum() - 1) < 0.001 && "Probabilities must sum up to 1"' failed.
   Aborted (core dumped)

2. How to improve future state estimation further ? I can see if I try and go ahead into future , the more error I am going to face. I have went through your 2nd suggestion in the above comment but still wanted to check if anything else we can do apart from noise of model.

[TomLKoller]

1. The error indicates, that the model probability calculation is erroneous. The sum of probabilites that are passed to weightedStateSum is not 1, which is an illness-check.

I do not think, that smoothing the future states is suited for an application. The reason to predict the future is, that the measurements of the future are unavailable. Smoothing requires the measurements of the future to be available. Thus, those concepts are contradictory.

2. Basically your prediction depends on the quality of your state estimate and your models. So thats where I would try to improve the prediction. i would take special care of the model probabilities as they influence the outcome heavily (if you have a 30% propability of the straight model while flying a curve, your prediction is more straight than it should be). Naturally, the prediction of the future gets worse the farther you try to go into the future (see e.g. weather forecasts). I assume that the prediction does only work well for multiple seconds if you have really good models and noises.

[akash901]

Thanks alot @TomLKoller for quick feedback. I totally agree, I am looking into models and their noises along with model probabilities in order to get better estimates. Current models which are implemented cover straight motion, constant turn. Along with these models I have seen some papers which have implemented constant acceleration, constant turn rate and acceleration models as well.

1. What are your thoughts about incorporating those models as well with current implementation in order to get better results universally for predicting aircraft future positions in all possible scenarios? Would they help in improving the results?

Updated Comment :

One more follow up comment. I removed Combination step from future prediction and got results pretty much near to ground truth values. Please check the attachment.

1. Would it be okay to skip combination while predicting future states (2 sec) in real scenario? I understand Combination performs weightedSum over MU and sigmas but its working correctly only after update call over filters in current implementation.

[TomLKoller]

@akash901 , your findings are a good start. You can use the models to estimate a greater variety of movements. The only problem is that more models increase the complexity of finding good noises and transition probabilities. I guess you have to find a tradeoff between the number of different models and the effort you want to invest to improve the result.

I would advise you to use a more complex flight path, if you want to find a method for all possible scenarios as the current simulation is quite simple. Depending on the UAV the possible maneuvers may be much more complex.

If you want to skip the combination step you still have to combine the estimates of the different filters (or find any policy to choose one). So what do you use as the filter output at the moment? If you use the imm.mu then you just get the filtered state of the current time, since the predict method does not change it.

I recommend you to implement your own combination step based on the weightedStateSum and weightedCovariance methods using the future predictions of the filters as input.

[akash901]

@TomLKoller Thank alot for your crisp and clear response. Yes I was using imm.mu for getting future state which isnt correct so my result were matching with current state. I second the thought of trying few complex manuvers for testing simulation robustly for current state estimation.

Regarding future state estimation combination step I am trying to find out good policy for correcting my results using weightedStateSum and weightedCovariance. For all future state estimate with current implemtation of combination stage results aren't accurate. Do you have any good paper or lead in this direction where I look into ?

[TomLKoller]

I'm afraid i haven't seen any paper regarding the combination step for future predictions. But i would assume there is work on that since predicting the flight path seems to be quite the obvious application of radar tracking.

[akash901]

Hello @TomLKoller , Sorry to getting back to you late. Recently I am trying to import Trajectory data in CSV recorded from external simulated radar into BoxPlus-IMM for prediction purpose. Track update rate in CSV is 3 secs, considering Doppler radar. Now when I am trying to predict state with deltaT = 3 secs (using function rts_imm.predictWithNonAdditiveNoise(deltaT)) I am facing error as shown below. Since my update rate from sensor would be 3 sec I want prediction of 3 secs in future. Please let me know if I am missing something. I can run state estimator with deltaT = 0.05 but cant run it with deltaT = 3. Is there any relation between deltaT and input data update rate?

FutureStateTest: /usr/local/include/ADEKF/ADEKF.h:201: void adekf::ADEKF<State>::predictFutureWithNonAdditiveNoise(DynamicModel, adekf::ADEKF<State>::SquareMatrixType<NoiseDim>&, const Controls& ...) [with int NoiseDim = 3; DynamicModel = constant_turn_model; Controls = {double}; State = Vector_State<double>; adekf::ADEKF<State>::SquareMatrixType<NoiseDim> = Eigen::Matrix<double, 3, 3>; typename adekf::StateInfo<State>::ScalarType = double]: Assertion!F.hasNaN() && "Differentiation resulted in an indeterminate form"' failed. Aborted (core dumped)`

[TomLKoller]

Hi @akash901,

In principle, this should work. When I am running my vector smooth example i can change deltaT to 3secs, but i have to disable smoothing. The results of the prediction seem to be so bad, that the covariance matrix gets indefinite during smoothing (even with my routine to assure positive definitiness. I have to look into that).

So maybe you have to tweak parameters to account for the higher delta T to get a working filter e.g. higher transition probabilities.

The error you get basically states, that the autodifferentation failed. This may happen if it gets an non-differentiable function or a undifferentiable working point e.g. sqrt(x) is non-differentiable for the autodifferentiation since 1/2 *1/sqrt(x) divides by 0. I can imagine two possible sources for your error:

1. Maybe your new dynamic models are non-differentiable in some points. Or i have not cleared all possible errors in my constant turn model.

If that is the case you have to either avoid non-differentiable working points or use the rule of lhopital to find the limit function of your working point (if it is only non-differentiable due to 0/0 division or infty/infty). See examples in my constant turn model for c1, c2 and c3.

2. Your custom predictFutureWithNonAdditiveNoise does something odd (The error log you are showing calls your predictFutureWithNonAdditiveNoise function

[akash901]

@TomLKoller Thanks for quick response. I am looking into the parameters. Also Please see attached screenshot of trajectory of aircraft with track update rate of 3 secs. Blue trajectory shows ground truth whereas orange shows predicted. I have not plotted trajectory after sensor measurement update since I am only interested in predicted trajectory.

My scenerio is, Doppler radar which would be updating target positions every 3 secs but I want to predict beforehand where target would be using future prediction and then over that predicted position I have some computations to follow.

Above results do follow pattern of helical trajectory of aircraft but they dont converge near to ground truth but maintain constant gap.

My tracker loop is as follows :

1. Interaction of RTS IMM
2. Prediction of future state (deltaT = 3 secs)
3. Combination
4. Storing predicted state for plotting as shown in above plot (orange color)
5. Restore old mu, sigma and model probabilities
6. Predict current state (deltaT = 3 secs)
7. Update
8. Combination

RMSE of current state = 0.83 RMSE for future state = 2499

My questions is Why my predictions are not converging with ground truth in above plot? What can be possible reasons?

[TomLKoller]

Presumably, the dynamic models are not capable of predicting the helical motion. May i ask what your application aims for? I have not seen such a trajectory, since aircraft tracking is more of an exception in my research.

[akash901]

@TomLKoller My application is to predict future state of air combat fighter aircraft which is capable of having abrupt motions. Helical motion was just for testing purpose of tracker in more challenging scenerios. I want to find future position of aircraft in order to point a air defence weapon accordingly in the direction before aircraft reaches to the future position. Essentially we are solving fire control problem where weapon has to look at future position of aircraft in order to destroy target with minimum ammunition.

[TomLKoller]

Dear akash901,

I apologize for the sudden change of pace. After seeing that flight trajectory, I feared that the application field is military.

Due to fundamental regulations of my university I am not allowed to participate in military research. Thus, I must not give you any more advice for your application.