sfwa/ukf

How to handle the second-ordered term in calculating the derivative term for state vector ?

Closed this issue · 0 comments

I have a question about using this fantastic library. The problem can be simplified as follows.

In the uniformly accelerated motion,
$$P_{t+1} = P_{t} + v_{t} \Delta{t} + \frac{1}{2}a_{t}\Delta{t}^2$$ $$v_{t+1} = v_{t}+a_{t}\Delta{t}$$ $$a_{t+1} = a_{t}$$

How to write the state derivative term for $P$ ?

using ProcessModelTestStateVector = UKF::StateVector<
    UKF::Field<Position, real_t>,
    UKF::Field<Velocity, real_t>,
    UKF::Field<Acceleration, real_t>
>;

template <> template <>
ProcessModelTestStateVector ProcessModelTestStateVector::derivative<>() const {
    ProcessModelTestStateVector temp;
    /* Position derivative. */
    temp.set_field<Position>(get_field<Velocity>());  // how to handle the acceleration term?

    /* Velocity derivative. */
    temp.set_field<Velocity>(get_field<Acceleration>());

    /* Velocity derivative. */
    temp.set_field<Acceleration>(0.);

    return temp;
}