Any nonlinear model tested?

[vesor]

Seems only WhiteNoiseAcceleration used in tests.
I tried to add an non linear model but seems the result is very bad.

https://github.com/vesor/bayes-filters-lib/tree/vdev
(You need to modify path in CMakeLists.txt in tests folder and the path in plot.py.)

Besides, any plan for non linear measurement model?

Thanks

[claudiofantacci]

Hi @vesor, thanks for reaching out. I’m out of office at the moment and I’ll have a look at your repo as soon as possible! 👍
We definitely have plan to add some “standard” non-linear measurement models and a way to ease the use of multiple sensors.
We have plan to do quite big changes soon and release a new version of the library.
Any suggestion are welcome! If you would like to see some nonlinear models or sensors just let us know and we will try to add them 🎉

[vesor]

The code is in https://github.com/vesor/bayes-filters-lib/blob/vdev/src/BayesFilters/src/NonLinearScalarModel.cpp

void NonLinearScalarModel::propagate(const Eigen::Ref<const Eigen::MatrixXd>& cur_states, Eigen::Ref<Eigen::MatrixXd> pred_states)
{
    // see example 15.1 in book Optimal State Estimation Kalman, H-infinity, and Nonlinear Approaches
    pred_states = (0.5 * cur_states.array() + 25 * cur_states.array() * (1 + cur_states.array().pow(2)).inverse()).matrix();
}

And I modified the test_SIS and test_UKF to replace WhiteNoiseAcceleration with NonLinearScalarModel.
I use plot.py to visualize the result. Black dot is target, light cross is UKF and dark cross is SIS.

After I make noise smaller by change
double tilde_q = 10.0f; to 1.0f
and replace utils::make_unique(std::move(simulated_state_model));
with make_unique(std::move(simulated_state_model),1,1);
The result looks a little better.

Another thing is current LinearModel hardcode for measurement of size 2.

    H_.resize(2, 4);
    H_ << 1.0, 0.0, 0.0, 0.0,
          0.0, 0.0, 1.0, 0.0;

Maybe you can change it for more general sizes?

[vesor]

I also tried the UKF sample from Opencv (with some modifications such as change measurement to linear).
https://github.com/vesor/bayes-filters-lib/blob/vdev/test/test_UKF_opencv/main.cpp
The result looks fine:

But after I change the code to remove the 8cos( 1.2(n-1) ) item in double x1 = 0.5x + 25( x/(xx + 1) ) + 8cos( 1.2*(n-1) ) + q; (I want to make it the same as I used in bfl, which dosen't have this item.)
The result becomes bad:

I am quite new to Bayes filters and I don't know why 8cos( 1.2(n-1) ) item matters.

[xEnVrE]

Hi @vesor,

I had a look at your code and I will try to help you with the nonlinear model from example 15.1 taken from Optimal State Estimation Kalman, H-infinity, and Nonlinear Approaches.

Adding the term 8 cos (1.2 * (k -1))

First of all, if you wish to add the final term of the model equation it is pretty easy. You just need to add a member k_ to the class NonLinearScalarModel, initialize it to 0 and then use it as follows within the method NonLinearScalarModel::propagate

Eigen::ArrayXd cos_input(1, cur_states.cols());
cos_input = 1.2 * k_;

pred_states = (0.5 * cur_states.array() + 25 * cur_states.array() * (1 + cur_states.array().pow(2)).inverse() + 8 * cos_input.cos()).matrix();

k_++;

Change the process noise sampling part

According to the book, the process noise should be zero mean Gaussian with assigned scalar variance.
For this reason you don't need the term 1.0/3.0 * std::pow(T_, 3.0), the sample time T and the evaluation of the square root of the matrix (that you probably found in the WhiteNoiseAcceleration class). In the end it suffices to have:

• 1x1 Q_ matrix initialized, in the constructor, to some variance that you can take from the constructor
• a slightly different implementation of the method getNoiseSample, i.e.

   MatrixXd rand_vectors(getOutputSize().first, num);
   for (int i = 0; i < rand_vectors.size(); i++)
       *(rand_vectors.data() + i) = gauss_rnd_sample_();
  
   return std::sqrt(Q_(0, 0)) * rand_vectors;
  

Using the class LinearModel

As you are probably aware of, according to your previous comments, the class LinearModel assumes a state vector of the form x, x_dot, y, y_dot, i.e. a 2D target with velocities and extracts the components x and y.

This class is mainly used for library tests and probably will be extended in order to deal with a variable state size and shape. However, what we usually do, while developing a filter for a specific application, is to write a specific measurement class for the problem at hand.

If you wish to test the scalar model taken from the book you should adapt the class LinearModel, or write a new one, in order to:

• change the H_ matrix, i.e. to be 1x1 matrix with value 1.0
• change the R_ matrix., i.e. to be a 1x1 matrix with value sigma_x ^ 2 (you can instead ignore the parameter sigma_y)

With these changes, you should be able to execute the test_UKF_nonlinear properly. You will find a figure with the results at the end of this comment.

Particles initialization

For the particle filter implementation, your implementation in test_SIS_nonlinear is almost ready except for the initialization part. In fact the class InitSurveillanceAreaGrid, again used only for library tests, assumes a 2D state vector with velocities.

In your case, you should provide a class that initializes a 1D state vector using, e.g., some initial state. This class inherits from ParticleSetInitialization. There is only one method to be implemented, namely the method initialize. In this method, you can access to the state of each particle using particles.state(), a Eigen::VectorXd. An example follows:

class ParticlesInitialization : public ParticleSetInitialization
{
public:
    ParticlesInitialization(const double& initial_state)
    {
        initial_state_.resize(1);
        initial_state_(0) = initial_state;
    }

    ~ParticlesInitialization()
    { };

protected:
    bool initialize(ParticleSet& particles) override
    {
        // ParticleSet.components contains the number of particles
        for (std::size_t i = 0; i < particles.components; i++)
            particles.state() = initial_state_;

        return true;
    }

    VectorXd initial_state_;
};

After this change, you should be able to obtain good results also from test_SIS_nonlinear.

This figure was obtained using your script plot.py. The outcome of the UKF is represented in red, while the outcome of the particle filter is represented in green.

If you need some code to start with, you can find an example, obtained from your code plus the changes that I have suggested, in this branch.

If you want to test the complete Example 15.1, i.e. also taking into account the nonlinear measurement model y = x ^ 2 + r, you will need to implement a class inheriting from AdditiveMeasurementModel, i.e. a class describing a measurement model, not necessarily linear, with a Gaussian additive noise component.

Let me know in case of further doubts 😄

[claudiofantacci]

Outcomes from this issue:

• Improve/generalize LinearModel class, see Improve/generalize LinearModel class #56.
• Improve/generalize InitSurveillanceAreaGrid class, see Improve/generalize InitSurveillanceAreaGrid class #57.

[vesor]

Thanks for the detailed help.
I downloaded your code from impl/vesor branch, build and run.
But the test_SIS_nonlinear just crash and testSIS_xxx.txt files are empty.
test_UKF_nonlinear can run but the result still bad.

Do you have time to make impl/vesor branch work?
I have a quick look at all your changes, but I can't find the root cause.

[vesor]

First, there is a small bug in your branch,
std::pair<bool, MatrixXd> LinearModel::getNoiseSample(const int num) const
{
MatrixXd rand_vectors(2, num);

the size should be 1 instead of 2, when using scalar model.

Second, all your code changes seems to be code clean up which won't affect the final result.
The only thing which matters is the 8cos( 1.2(n-1) ) item.
After I added this item, I can get a good result as you have. (code with minimal change )

So bfl looks fine and the question become why the filter works bad if remove 8cos( 1.2(n-1) ) item.
That may be out of bfl's scope but I appreciate if you can share your thoughts.

Thanks.

[claudiofantacci]

Hi @vesor, thanks for your feedback.
We are substantially upgrading the library for the next release and we are including your feedback. Thanks about that 👍

So bfl looks fine and the question become why the filter works bad if remove 8cos( 1.2(n-1) ) item.
That may be out of bfl's scope but I appreciate if you can share your thoughts.

It is indeed something related to the filtering problem at hand and not about the library.
I don't have the book and hence cannot read the problem at this very moment, but I will discuss it with @xEnVrE and let you know our opinion 😄

If you have any suggestion for us to improve our work, let us know!

[xEnVrE]

Closing, feel free to re-open.