Vehicle simulation headache

Hello and sorry for the long post, I'm trying to implement my first car simulation stuff. I've been through Marco Monster's tutorial and also Physics of Racing Series and most of the Racer.nl docs.
For the sake of simplicity I'm starting in 2d, using SAE conventions (X pointing forward, Y to the right)

In a nutshell this is my simulation step:
1. Calculate current engine RPM from driven wheel rotation (with limiter and autoclutch)
2. Calculate drive torque on driven wheels and brake torques
3. Calculate forces and traction torques at each wheel with Pacejka MF (same method for all wheels)
4. Accumulate wheel torques and calculate angular velocities
5. Accumulate body forces (from wheel forces) and resistance
6. Accumulate torques on body caused by the wheels' forces
7. Calculate body movement and angular velocity
8. Dynamic weight transfer

The straight line acceleration and braking seems to be working;
I tried with different timesteps between 1000 and 20000 Hz. Even at 1000Hz the wheels seem to be fine, no jittering, not even at low speed 1-2km/h. The undriven wheels are also rolling because of the torque generated by the negative slip ratio, so that part seems to be ok.

My problem starts with steering, and I'm not sure I'm calculating with the right force values.

Here are some simplified code;
Calculate tyre forces:
[source lang="cpp"]
for (int i=0;i&lt4i++) {
// Calculate wheel velocity according to body movement and rotation
VHFLOAT flen=...; // Lenghth of the force arm (Wheel distance from CM)
VHVECTOR vrolln=...; // Turning force direction vector (unit length) - perpendicular to the vector from CM to the wheel

// Calculate the wheel velocity in CAR space
VHVECTOR vel=car.vel;
Transform_World_Car(vel);
vel.x+=flen*car.body_ang_vel*vrolln.x;
vel.y+=flen*car.body_ang_vel*vrolln.y;

// Transform the velocity vetor to the current wheel's local space
Transform_Car_Wheel(vel,i);

// Calculate slip ratio and slip angle
VHFLOAT wheelspeed=car.wheels.roll_ang_vel*car.wheels.radius;
if (VHABS(vel.x)&lt0.0000001) {
if (VHABS(wheelspeed)>0.1) car.wheels.slip_ratio=0.01*VHSGN(wheelspeed);
else car.wheels.slip_ratio=0;
car.wheels.slip_angle=car.wheels.steer_angle;
if (VHABS(vel.y)>0.1) car.wheels.slip_angle+=(vel.x/vel.y);
} else {
car.wheels.slip_ratio=(wheelspeed-vel.x)/VHABS(vel.x);
if (car.wheels.slip_ratio>3) car.wheels.slip_ratio=3;
car.wheels.slip_angle=VHATAN(vel.y/vel.x)+car.wheels.steer_angle;
}

// Calculate pacejka values
CPacejka pac;
TPacejkaParameters pars;
pars.fz=car.wheels.fz/1000.0;
pars.slip_ratio_percent=car.wheels.slip_ratio*100.0;
pars.slip_angle_degree=car.wheels.slip_angle*(180.0/PI);
pars.camber_angle_degree=car.wheels.camber;
pac.Calculate(&car.wheels.tyre_magic_values,&pars,car.wheels.tyre_forces);

// Calculate traction torque from longitudinal force
car.wheels.road_torque=-car.wheels.tyre_forces.fx*car.wheels.radius;
}
[/source]
Calculating the total forces on body. This is where I'm not sure about tyre forces. I don't cap to the friction circle, only setting the FX and FY into a vector and summing them for all wheels.
[source lang="cpp"]
VHVECTOR f_total=VHVECTOR(0,0);

// Wheel forces
for (int i=0;i&lt4i++) {
VHVECTOR f_wheel;
f_wheel.x=car.wheels.tyre_forces.fx;
f_wheel.y=car.wheels.tyre_forces.fy;

Transform_Wheel_Car(f_wheel,i);
f_total.x+=f_wheel.x;
f_total.y+=f_wheel.y;
}
Transform_Car_World(f_total);

// Drag
VHVECTOR f_drag=...;
// Rolling resistance
VHVECTOR f_rr=...;

car.total_forces.x=f_total.x+f_drag.x+f_rr.x;
car.total_forces.y=f_total.y+f_drag.y+f_rr.y;
[/source]

And finaly accumulating the forces and torques on the body:
[source lang="cpp"]
// --- Position ---
car.acc.x=car.total_forces.x/car.mass;
car.acc.y=car.total_forces.y/car.mass;
... // Integrate velocity and position

// --- Orientation ---
car.body_ang_acc=0;
for (int i=0;i&lt4i++) {
// Wheel forces in CAR space
VHVECTOR f_wheel;
f_wheel.x=car.wheels.tyre_forces.fx;
f_wheel.y=car.wheels.tyre_forces.fy;
Transform_Wheel_Car(f_wheel,i);

// Using only the force component that is perpendicular to the (CM->Wheel) vector
// Calculate the length of the CM->Wheel vector (length of the force arm)
VHFLOAT flen=...;
// Calculate a (unit lenght) vector perpendicular to the CM->Wheel vector
VHVECTOR varmn=...;

// Finaly the force component that is perpendicular to the force arm
VHFLOAT f_roll=varmn.x*f_wheel.x+varmn.y*f_wheel.y;

// The torque caused by this wheel
VHFLOAT t=f_roll*flen;
// Calculate angular acceleration
car.turn_ang_acc+=t/car.inertia;
}

... // Integrate angular velocity and orientation[/source]


But somehow the code above doesn't work even with very small steering angle (1degree)
Using "controlled" values in the body torque calculation works fine;
using constant 20N at the front wheels and -20N at the rear turns the body in the right direction.

The lateral Pacejka generates around 1-2kN which seems to be good, and the signs for front and rear tyre values are also good.

Using 0.01 throttle factor which is a very very very little acceleration, the body rotating in the right direction but much faster, if I add more throttle 0.1 which is still little acceleration (the slip ratio is still in acceptable range - no spinning tyre) the car body goes crazy.
Not jittering and no sign of instability, it just turns back and forth.

This is how I think it should work (with constant engine torque) not exact force values, just to demonstrate:
Step1: Right turn 1degree, front wheels generate +1kN, rear wheels 0kN lateral forces
Step2: Body has positive ang_vel, front wheels have 200N, rear wheels -2kN because of the body turns the slip angle is 60-90degrees

And as a simplified model, step1 starts turning the car right and step2 generates a counter torque, and their difference will make the car turn depending on the steering angle.


Do you guys have any idea? Thanks in advance!