I am making a particle filter implementation using the kinematic equations as my state transition function, that is to say:
$$
\dot{p} = v\\
\dot{v} = a
$$
where $a$ is assumed to be given. My first attempt was using the backward Euler method:
$$
p_{k+1} = p_k + T_s v_k\\
v_{k+1} = v_k + T_s a_k
$$
for $T_s = 0.01$ and $|a| \leq 0.5$. The velocity plot is more or less perfect, but there is substantial error between actual position, found by MATLABs cumtrapz, and the Euler method derived position.
Plot was created roughly by
p = zeros(N,1);
v = zeros(N,1);
for i=2:N
p(k) = p(k-1) + Ts*v(k-1);
v(k) = v(k-1) + Ts*a(k-1);
end
Decreasing the sampling time has virtually 0 impact.
I have read online about Euler method being a first order discretization method so I figure it has something to do with that given that this is a second order set of ODEs?
I figure I should look into higher order solvers, e.g., as an alternative. Given this is a particle filter implementation the solution method needs to be quite computationally cheap.
Is my understanding of the problem completely incorrect? Also is using ERK a viable solution? Or is the problem here somewhere else.
Note: plots made using MATLAB