Include yaw angle in KALMAN FILTERING

Projects General Guidance
1

Hello Everyone!

Ive done a Kalman filter for integrate a GPS and IMU sensors, I did it to get the position and velocity in x and y directions, getting the aceleration from the IMU and the velocity and position from the GPS.

Now I'm trying to include a 3rd axis with yaw angle, to see the car yaw rate in the XY plane, do you have any idea of how to implement ti?

do't know if this dinamic model is valid:

yaw = yaw(0) + yaw_rate*dt
yaw_rate=yaw_rate

[yaw ] = [1 0] * [yaw] + [dt] * [yaw_rate]
[yaw_rate] [0 1] [yaw_rate] [0]

x = Ax + Bu

yaw_rate from IMU
yaw angle from GPS or IMU...

If you could help me with this, I would be really gratefull
Thank you!

2

A Kalman filter generally requires an NxN matrix, where N is the number of state variables that enter into the system model. What you are using for a system model isn't clear, (I can't make sense of what appears in your post) but for 2D position, velocity and orientation, you would need at least a 5x5 matrix.

For the theory, I strongly recommend Dan Simon's excellent and very readable textbook "Optimal State Estimation". Here is an introductory article that he wrote.

Although not applicable to your problem, by far the best open source Kalman filter for Arduino is RTIMUlib. Worth some study!