GPS + IMU data and kinematics equations

Question

I have the following data

• Longitudinal acceleration, $a_x^{IMU}$
• Lateral acceleration, $a_y^{IMU}$
• Vertical acceleration, $a_z^{IMU}$
• Yaw angle, $\psi$
• Yaw rate, $\dot{\psi}$
• Latitude, $\rightarrow Y$ ( UTM)
• Longitude, $\rightarrow X$ ( UTM)
• Speed, $v$

Sampled from an IMU+GPS installed in a car at $10Hz$.

I want to define the motion equations in order to use a Kalman filter.

I've read this post, assuming $a_y=0$ (and $\dot{\psi} = 0)$, I was able to use the filter using these equations $$ \begin{cases} s_{x, t+1} = s_{x,t} + v_{t} \cos(\psi) \, dt + \frac{1}{2} a_{x, t}^{IMU} \cos(\psi) (dt)^2\\ s_{y, t+1} = s_{y,t} + v_{t} \sin(\psi) \, dt + \frac{1}{2} a_{x, t}^{IMU} \sin(\psi) (dt)^2\\ v_{t+1} = v_{t} + a_{x, t}^{IMU} \, dt \\ a_{x, t+1} = a_{x, t}^{IMU} \end{cases} $$ where $(s_x, s_y)$ are observed though $(X, Y)$.

---

How can I generalize the previous equations with $a_y^{IMU} \ne 0$? Moreover, how can I involve the yaw rate?

From Wikipedia the acceleration of a material point is given by $$ \vec{a} = \frac{dv}{dt}\hat{u}_t + \frac{v^2}{R}\hat{u}_n. $$

Am I right to assume $$ \frac{dv}{dt} = a_x^{IMU} $$ and $$ \frac{v^2}{R} = \dot{\psi} \, v = a_y^{IMU} \ \text{?} $$

In particular, from Performance Vehicle Dynamics the lateral acceleration is given by the sum of two components $$ a_Y = V \dot{\psi} + \dot{V_Y} $$ Why the $a_Y$ above is the sum of two terms? $V \dot{\psi}$ and $\dot{V_Y}$ (contrary to the acceleration of a material point equation)? How this equation is related to the $a_y^{IMU}$ data?

Thanks in advance!

Answer 1

Why the $a_Y$ above is the sum of two terms?

From the article:

However, the car also has a horizontal side-slip velocity

The vehicle "lateral" vector $Y$ points perpendicular to the body frame, and thus the direction of travel and accel could be a combination of the forward and lateral velocities. The car is experiencing both "centripetal" accel to make it turn, and may be experiencing lateral accel as it slides perpendicularly.

Ynlike a point mass, which always travels "forward" and accelerates "sideways" and "forwards".

These [two] components are called the tangential acceleration and the normal or radial acceleration

---

To update when $a_y\neq 0$, you need to update all four of those equations to include a lateral velocity and accel. It will look almost like your accel and vel terms, but will be perpandicular (instead of $cos$ and $-sin$, you have $-cos$ and $sin$ as a unit vector).

---

And, you'll want to take a close look at $x$ and $y$ in your state equations. It appears from the third / fourth equation that $x$ is body fixed, whereas from the first two $x$ and $y$ are synonymous with Lat/Lon (e.g., earth fixed).

That's fine if $v$ is actually speed, which it is for a forward-accel-only vehicle, but with lateral accel, $v$ needs to be a vector of two velocities. After breaking apart velocity $(v_x(t), v_y(t))$ and accel $(a_x(t), a_y(t))$ and deciding on a common $x$ and $y$ for all equations, it will be easier to rewrite those state transition equations.