Here example how to get Yaw (azimuth) of car.
Sorry that code written in object pascal, have no time to translate...
TMatrix33 = array[0..2] of array[0..2] of Single;
BufferRet = array[0..BufferRetLen] of Byte;
TVector = record
X, Y, Z: Single;
end;
LocMat : TMatrix33;
eul : TVector;
FB : BufferRet; // our buffer
sinx, cosx : single;
Movememory(@sinx, @FB[56], 4);
Movememory(@cosx, @FB[64], 4);
LocMat[0,0]:=cosx;
LocMat[0,1]:=0;
LocMat[0,2]:=-sinx;
LocMat[1,0]:=0;
LocMat[1,1]:=1;
LocMat[1,2]:=0;
LocMat[2,0]:=sinx;
LocMat[2,1]:=0;
LocMat[2,2]:=cosx;
eul := CalcEulerM33(@LocMat); // direction in euler
Grad := eul.Y*(180/pi); // azimuth in degrees
function CalcEulerM33(M:PMatrix33) : TVector;
begin
if (M[0,1] > 0.9998) then // singularity at north pole
begin
Result.Y := arctan2(M[2,0], M[2,2]);
Result.Z := PI/2;
Result.X := 0;
exit;
end
else if (M[1,0] < -0.9998) then // singularity at south pole
begin
Result.Y := arctan2(M[2,0], M[2,2]);
Result.Z := -PI/2;
Result.X := 0;
exit;
end;
Result.Y := arctan2(-M[0,2], M[0,0]);
Result.Z := arcsin(M[0,1]);
Result.X := arctan2(-M[2,1],M[1,1]);
end;