Forward kinematics data modeling

I have built a simple robotic arm using 3 RC Servos and an Arduino.

I just want to play around with it and learn something about robotics.

Currently, I am trying to compute the position of the tip of the robotic arm using the three angular positions of the servos.

"Forward kinematics" I think is the technical term for this.

Btw the tip of the arm is a pen, I thought I might try to draw soemthing with it later on.

In the movement range of the arm I set up a cartesian coordinate system and recorded 24 (angle => position) samples.

table header: servo1, servo2, servo3, x, y, z 111,61,33,0,0,0 96,50,71,0,0 - Pastebin.com

Now, I am trying to model this data, but I am a bit out of my depth here.

Here is my approach so far:

I use the Denavit–Hartenberg equations found on Wikipedia Denavit?Hartenberg parameters - Wikipedia, the free encyclopedia.

I then try to determine the parameters using least squares optimization.

Code:

`minimize(sum(norm(f(x,P)-y)^2))`

I also added linear terms to the input and output of the model to compensate for possible distortions (e.g. phase-shift in the servo angle):

My Python code: [Python] Forward kinematics optimization - Pastebin.com

But it just won't work, none of my attempts have converged on a good solution.

I also tried a simple 3x4 matrix multiplication, which tbh does not make much sense as a model, but oddly it didnt do worse than the more sophisticated model above.

I hope there is some smart guy out there who can help. Thanks in advance.