# GPS + IMU data and kinematics equations

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?

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.