Search Results (Searched for: )
- Tinker
- Tinker
21 Feb 2025 11:58
7I97T joint following error was created by Tinker
7I97T joint following error
Category: General LinuxCNC Questions
Not sure if I'm posting this in the right place but I cant get past the joint following error to tune my -10/10 volt servos. I've reloaded everything several times with the same result... When I enable the drives the servos start drifting for about .001/.002 and then the following error. I am THE most computer illiterate person on earth and have no idea how to fix this thing. I've been trying for the last couple of months to get it going but can't get past this. With the help from this forum I've been able to get the network, mesa flash, mesa ct and card wiring complete but am stuck on this one.
- denhen89
21 Feb 2025 11:48 - 21 Feb 2025 12:03
Replied by denhen89 on topic (LPT) Debian 12 Latency spikes/unexpected real time delay. Looking for solution.
(LPT) Debian 12 Latency spikes/unexpected real time delay. Looking for solution.
Category: General LinuxCNC Questions
Thank you for that.
Update: I have the pc at this moment at home, so i can do some tests while doing my work.
When i start latency-histogram and run it for some time (e.g. 15 min) with 3x glxgears the base thread looks good. Depending on the cpu bios settings i have stable max us of 20.000, but i found out that when i addionally start a youtube video (720p), the latency goes up, pretty quickly, almost directly after starting the video and then the latency jumps to 45000 then 60.000 and who knows how high it goes but then of course it doesnt matter anymore.
Of course i have to connect the pc back to the machine and see if it starts without unexpected real time delay and if the machines runs smooth, because after going from wheezy to debian 12 i had the situations where there latency-test (not histogram) showed good results in for a short time but the motors didnt move as they should.
While writing this post, the latency-histogram is still runing and still at 20.0 us. Maybe i am able to get it to work, but it seems that the vga might cause the latency spike and thats why i get a unexpected real time delay on start up.
What i could do next is to buy better ram, because one stick of the ddr3 1600mhz (2x4gb) ram that i had from my last main pc is not working, so i am currently using some old 1333mhz ddr3 2x2gb rams.
Also a onboard vga might give better results, but i dont have a pc with integrated vga to test it.
Update: I have the pc at this moment at home, so i can do some tests while doing my work.
When i start latency-histogram and run it for some time (e.g. 15 min) with 3x glxgears the base thread looks good. Depending on the cpu bios settings i have stable max us of 20.000, but i found out that when i addionally start a youtube video (720p), the latency goes up, pretty quickly, almost directly after starting the video and then the latency jumps to 45000 then 60.000 and who knows how high it goes but then of course it doesnt matter anymore.
Of course i have to connect the pc back to the machine and see if it starts without unexpected real time delay and if the machines runs smooth, because after going from wheezy to debian 12 i had the situations where there latency-test (not histogram) showed good results in for a short time but the motors didnt move as they should.
While writing this post, the latency-histogram is still runing and still at 20.0 us. Maybe i am able to get it to work, but it seems that the vga might cause the latency spike and thats why i get a unexpected real time delay on start up.
What i could do next is to buy better ram, because one stick of the ddr3 1600mhz (2x4gb) ram that i had from my last main pc is not working, so i am currently using some old 1333mhz ddr3 2x2gb rams.
Also a onboard vga might give better results, but i dont have a pc with integrated vga to test it.
- rodw
21 Feb 2025 11:34
Replied by rodw on topic EtherCAT plasma torch voltage reader
EtherCAT plasma torch voltage reader
Category: Show Your Stuff
Perfect! Not sure what plasma cutter you have but if it was say hypertherm, a surface mount circular connector that just pushes into the socket on the machine and you would have a nice complete self mounting package. I used the 7i76e spindle relay for torch on so there is nothing wrong with your photo diode. You just need to add a pull down resistor on the arcOK as discussed in the qtplasmac docs.
Of course if you had a XPR hi def its Ethercat anyway!
Of course if you had a XPR hi def its Ethercat anyway!
- rodw
21 Feb 2025 11:16
Replied by rodw on topic Red Machine Outline is backwards in Y dimension
Red Machine Outline is backwards in Y dimension
Category: Qtvcp
Really think of the X,Y graphs you drew at school. You can choose any quadrant but its much easier playing in the positive quandrant which places 0,0 at the bottom left and you play in the top right quandrant (Travel +X,+Y). It's easier to have home sensors near 0,0 but I do have a machine that was easier to put the y home sensor at max Y so I have a very long home_offset to move it back to the 0,0 position. I have thought of changing the setup so I work in the bottom right quadrant so 0,0 is at top left close to the home switch. (travel would be +X, -Y) You never work in raw coordinates as you touch off your g54 offset so 0,0 is somewhere on your workpiece. If you touched off at bottom left of your workpiece, you would be working on the workpiece in that positive quandrant again. Sometimes you would touch off in the centre of your workpiece to bore a hole or make a motor mount, Sometimes you might also touch off at the back left against the corner of your vice jaws if you had a few parts to make. (travel +X, -Y). If I do this, I have a big magnet I stick on the jaws so I have a repeatable position.
- JuFu
- JuFu
21 Feb 2025 10:50
Replied by JuFu on topic Switching HAL pin in subroutine (macro)
Switching HAL pin in subroutine (macro)
Category: O Codes (subroutines) and NGCGUI
Issue solved by using M100
- rodw
21 Feb 2025 10:48
Replied by rodw on topic Building a 3-axis plasma table with mesa 7i96s, THCad-2 and nema23 steppers
Building a 3-axis plasma table with mesa 7i96s, THCad-2 and nema23 steppers
Category: Show Your Stuff
I've not used it in earnest but QTplasmac installs fine with 4 axes with 2 motors on the gantry, XYYZA
I have installed it with 5 stepper drives though
Probably pair it up with Sheetcam rotary for CAM
I have installed it with 5 stepper drives though
Probably pair it up with Sheetcam rotary for CAM
- Daan96
- Daan96
21 Feb 2025 10:31
Replied by Daan96 on topic Building a 3-axis plasma table with mesa 7i96s, THCad-2 and nema23 steppers
Building a 3-axis plasma table with mesa 7i96s, THCad-2 and nema23 steppers
Category: Show Your Stuff
Then the choice seems quickly made; I'll have to get my hands on an old PC and start exploring Linux, which is completely new to me. Do you have it running on an old PC mounted near the table, or is it also possible to run Linux on a NUC?
In any case, I'll dive into the various posts on this forum while continuing with the mechanical design.
I see that QTPlasm doesn't work so easily for the rotation axis—perhaps I should leave it out for the first project and later build it as a standalone machine, just like Tommy did.
Thanks for the input! It's greatly appreciated!
In any case, I'll dive into the various posts on this forum while continuing with the mechanical design.
I see that QTPlasm doesn't work so easily for the rotation axis—perhaps I should leave it out for the first project and later build it as a standalone machine, just like Tommy did.
Thanks for the input! It's greatly appreciated!
- JuFu
- JuFu
21 Feb 2025 10:16
Switching HAL pin in subroutine (macro) was created by JuFu
Switching HAL pin in subroutine (macro)
Category: O Codes (subroutines) and NGCGUI
I want to set / reset a HAL pin by using G-codes (switching a pneumatic valve on/off).
In the documentation G-code Overview | 4. HAL pins and INI values is written: " ... the only way to set HAL pins from G-code remains M62-M65, M67, M68 and custom M100-M199 codes."
I wrote the below subroutine (macro):
o<test> sub
(MSG, Spannzange öffnen)
M62 #<_hal[hm2_7i76e.0.7i76.0.0.output-00]
o<test> endsub
M2
When I run the macro, the response is: "Benannter Parameter nicht beendet"
(Using gmoccapy Ver 3.4.9 in German language)
Is there a better way to control HAL pins?
If not, what do I need to do?
Juergen
In the documentation G-code Overview | 4. HAL pins and INI values is written: " ... the only way to set HAL pins from G-code remains M62-M65, M67, M68 and custom M100-M199 codes."
I wrote the below subroutine (macro):
o<test> sub
(MSG, Spannzange öffnen)
M62 #<_hal[hm2_7i76e.0.7i76.0.0.output-00]
o<test> endsub
M2
When I run the macro, the response is: "Benannter Parameter nicht beendet"
(Using gmoccapy Ver 3.4.9 in German language)
Is there a better way to control HAL pins?
If not, what do I need to do?
Juergen
- Grotius
21 Feb 2025 10:10 - 21 Feb 2025 13:04
Replied by Grotius on topic scurve trajectory planner
scurve trajectory planner
Category: General LinuxCNC Questions
@Arciera,
That's what i think we don't need. If the dogleg solver is able to find the endpoint in 3d space. Why do the tranformations
as used in the abstract?
We have already a case where the helix tail or start from a G2, G3 is used as end vector.
I see you have splitted up the gaus interpolation into x, y, z.
Edit:
Did a mayor update on the infrastructure.
- Use struct for clothoid data.
- Use seperate plot function.
- Use enum to choose a solver from the list.
- Use a central residual function used by all solvers. Where the objective function can cast types.
That's what i think we don't need. If the dogleg solver is able to find the endpoint in 3d space. Why do the tranformations
as used in the abstract?
We have already a case where the helix tail or start from a G2, G3 is used as end vector.
I see you have splitted up the gaus interpolation into x, y, z.
Edit:
Did a mayor update on the infrastructure.
- Use struct for clothoid data.
- Use seperate plot function.
- Use enum to choose a solver from the list.
- Use a central residual function used by all solvers. Where the objective function can cast types.
- Aciera
21 Feb 2025 10:06
Replied by Aciera on topic scurve trajectory planner
scurve trajectory planner
Category: General LinuxCNC Questions
As i understand it there should be no need for another transformation besides the one described in the paper.
- JB-Motoring
- JB-Motoring
21 Feb 2025 10:00
Replied by JB-Motoring on topic Mesa 7i97T + 7i84 + 7i78 configuration
Mesa 7i97T + 7i84 + 7i78 configuration
Category: General LinuxCNC Questions
Hey guys,
so I got Linuxcnc running- And I would like to test all i o 's on the bench before moving to the machine.
As soon as I hit enable (F1/F2) all assigned Analog Outputs show -10V.
There is no encoder or drive connected, just the naked Mesa on the desk.
How can I handle this to see, if the output is working?
Thanks guys!
so I got Linuxcnc running- And I would like to test all i o 's on the bench before moving to the machine.
As soon as I hit enable (F1/F2) all assigned Analog Outputs show -10V.
There is no encoder or drive connected, just the naked Mesa on the desk.
How can I handle this to see, if the output is working?
Thanks guys!
- Grotius
21 Feb 2025 09:58
Replied by Grotius on topic scurve trajectory planner
scurve trajectory planner
Category: General LinuxCNC Questions
@Arciera,
Indeed. Good spot !!!
I was thinking about the 3d fitting between gcode segments.
Do we really need the transformation, and then do the fitting in a null plane?
This was needed for a 2d clothoid, when used in a 3d plane. But for this i think
we don't needed, unless there is something i forgot.
I thought maybe we can just fit the clothoids in 3d space.
Indeed. Good spot !!!
I was thinking about the 3d fitting between gcode segments.
Do we really need the transformation, and then do the fitting in a null plane?
This was needed for a 2d clothoid, when used in a 3d plane. But for this i think
we don't needed, unless there is something i forgot.
I thought maybe we can just fit the clothoids in 3d space.
- Aciera
21 Feb 2025 09:56
Replied by Aciera on topic scurve trajectory planner
scurve trajectory planner
Category: General LinuxCNC Questions
Here is my failed attempt:
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlinear.h>
/* eq18, page 4.
*
* stot = total clothoid length
* s = global length at clothoid to retrieve sharpness c.
*
* Retrieve the sharpness for s.
*
*/
void eq18(const double s, const double stot,
const double sharpnessi0, const double sharpnessi1, const double sharpnessi2, const double sharpnessi3,
const double yi1, const double yi2, const double yi3, const double yi4,
double *sharpness_at_s) {
double s1 = stot / 4.0;
double s2 = s1 + s1;
double s3 = s1 + s1 + s1;
double s4 = stot;
if (s <= s1) {
*sharpness_at_s = sharpnessi0 + yi1 * s;
} else if (s > s1 && s <= s2) {
*sharpness_at_s = sharpnessi1 + yi2 * (s - s1);
} else if (s > s2 && s <= s3) {
*sharpness_at_s = sharpnessi2 + yi3 * (s - s2);
} else if (s > s3 && s <= s4) {
*sharpness_at_s = sharpnessi3 + yi4 * (s - s3);
} else {
// Handle the case where s is out of bounds (optional)
*sharpness_at_s = 0.0; // or some other default value
printf("error: eq18. \n");
}
}
void eq18_single_clothoid(const double s,
const double sharpnessi0,
const double yi1,
double *sharpness_at_s) {
*sharpness_at_s = sharpnessi0 + yi1 * s;
}
/* eq19, page 4.
*
* stot = total clothoid length
* s = global length at clothoid to retrieve sharpness c.
*
* Retrieve the kappa for s.
*
*/
void eq19(const double s, const double stot,
const double sharpnessi0, const double sharpnessi1, const double sharpnessi2, const double sharpnessi3,
const double kappai0, const double kappai1, const double kappai2, const double kappai3,
const double yi1, const double yi2, const double yi3, const double yi4,
double *kappa_at_s) {
double s1 = stot / 4.0;
double s2 = s1 + s1;
double s3 = s1 + s1 + s1;
double s4 = stot;
if (s <= s1) {
*kappa_at_s = kappai0 + sharpnessi0 * s + 0.5 * yi1 * (s * s);
} else if (s > s1 && s <= s2) {
*kappa_at_s = kappai1 + sharpnessi1 * (s - s1) + 0.5 * yi2 * ((s - s1) * (s - s1));
} else if (s > s2 && s <= s3) {
*kappa_at_s = kappai2 + sharpnessi2 * (s - s2) + 0.5 * yi3 * ((s - s2) * (s - s2));
} else if (s > s3 && s <= s4) {
*kappa_at_s = kappai3 + sharpnessi3 * (s - s3) + 0.5 * yi4 * ((s - s3) * (s - s3));
} else {
// Handle the case where s is out of bounds (optional)
*kappa_at_s = 0.0; // or some other default value
printf("error: eq19. \n");
}
}
void eq19_single_clothoid(const double s,
const double sharpnessi0,
const double kappai0,
const double yi1,
double *kappa_at_s) {
*kappa_at_s = kappai0 + sharpnessi0 * s + 0.5 * yi1 * (s * s);
}
/* eq20, page 4.
*
* stot = total clothoid length
* s = global length at clothoid to retrieve sharpness c.
*
* Retrieve the theta at s.
*
*/
void eq20(const double s, const double stot,
const double sharpnessi0, const double sharpnessi1, const double sharpnessi2, const double sharpnessi3,
const double kappai0, const double kappai1, const double kappai2, const double kappai3,
const double thetai0, const double thetai1, const double thetai2, const double thetai3,
const double yi1, const double yi2, const double yi3, const double yi4,
double *theta_at_s) {
double s1 = stot / 4.0;
double s2 = s1 + s1;
double s3 = s1 + s1 + s1;
double s4 = stot;
if (s <= s1) {
*theta_at_s = thetai0 + kappai0 * s + (0.5*sharpnessi0) * (s * s) + (1.0 / 6.0) * yi1 * (s * s * s);
} else if (s > s1 && s <= s2) {
*theta_at_s = thetai1 + kappai1 * (s - s1) + (0.5*sharpnessi1) * ((s - s1) * (s - s1)) + (1.0 / 6.0) * yi2 * ((s - s1) * (s - s1) * (s - s1));
} else if (s > s2 && s <= s3) {
*theta_at_s = thetai2 + kappai2 * (s - s2) + (0.5*sharpnessi2) * ((s - s2) * (s - s2)) + (1.0 / 6.0) * yi3 * ((s - s2) * (s - s2) * (s - s2));
} else if (s > s3 && s <= s4) {
*theta_at_s = thetai3 + kappai3 * (s - s3) + (0.5*sharpnessi3) * ((s - s3) * (s - s3)) + (1.0 / 6.0) * yi4 * ((s - s3) * (s - s3) * (s - s3));
} else {
// Handle the case where s is out of bounds (optional)
*theta_at_s = 0.0; // or some other default value
printf("error: eq20.\n");
}
}
/* Get the theta given s (length) for a single clothoid.
*
*/
void eq20_single_clothoid(const double s,
const double sharpnessi0,
const double kappai0,
const double thetai0,
const double yi1,
double *theta_at_s) {
*theta_at_s = thetai0 + kappai0 * s + (0.5*sharpnessi0) * (s * s) + (1.0 / 6.0) * yi1 * (s * s * s);
}
/* parameters to model */
struct model_params
{
double t10;
double t20;
double k10;
double k20;
double c10;
double c20;
double xs;
double ys;
double zs;
double xe;
double ye;
double ze;
double xi;
double yi;
double zi;
};
/* eq14, page 4
*
* Most efficient and precise solution to compute the clothoid curve lenght.
* Using gauss legendre, up to 20 points. Use a 30 point legendre to get even
* more accuracy.
*
* This calculation is more accurate then the above eq14_drift function.
*
* s = lenght on clothoid to retrieve the xyz point, xi,yi,zi.
* x0 y0 z0 = Clothoid start point.
* ltot = Clothoid length up to s.
*
* gamma1, gamma2, clothoid's rigid body factor. Synonims for y11, y22.
*
*/
float eq14_gauss_x(double s,
double x0, double xe,
double theta10, double theta20,
double kappa10, double kappa20,
double sharpness10, double sharpness20,
double gamma1, double gamma2) {
// Gauss-Legendre 20-point weights & nodes
const double gauss_weights[20] = {
0.1527533871, 0.1491729865, 0.1420961093, 0.1316886384, 0.1181945319,
0.1019301198, 0.0832767416, 0.0626720483, 0.0406014298, 0.0176140071,
0.0176140071, 0.0406014298, 0.0626720483, 0.0832767416, 0.1019301198,
0.1181945319, 0.1316886384, 0.1420961093, 0.1491729865, 0.1527533871
};
const double gauss_nodes[20] = {
-0.9931285992, -0.9639719273, -0.9122344283, -0.8391169718, -0.7463319065,
-0.6360536807, -0.5108670019, -0.3737060887, -0.2277858511, -0.0765265211,
0.0765265211, 0.2277858511, 0.3737060887, 0.5108670019, 0.6360536807,
0.7463319065, 0.8391169718, 0.9122344283, 0.9639719273, 0.9931285992
};
double sum_x = 0.0;
for (int i = 0; i < 20; i++) {
double weight = 0.5 * s * gauss_weights;
// Calculate the clothoid theta, kappa, sharpness at current s.
double sharpness10_i;
eq18_single_clothoid(0,sharpness10,gamma1,&sharpness10_i);
double sharpness20_i;
eq18_single_clothoid(0,sharpness20,gamma2,&sharpness20_i);
double kappa10_i;
eq19_single_clothoid(0,sharpness10_i,kappa10_i,gamma1,&kappa10_i);
double kappa20_i;
eq19_single_clothoid(0,sharpness20_i,kappa20_i,gamma2,&kappa20_i);
double theta10_i;
eq20_single_clothoid(0,sharpness10_i,kappa10_i,theta10,gamma1,&theta10_i);
double theta20_i;
eq20_single_clothoid(0,sharpness20_i,kappa20_i,theta20,gamma2,&theta20_i);
// Compute new coordinates using Gauss quadrature integration
double dx = cos(theta10_i) * cos(theta20_i) * weight;
sum_x += dx;
}
return x0 + sum_x - xe;
}
float eq14_gauss_y(double s,
double y0, double ye,
double theta10, double theta20,
double kappa10, double kappa20,
double sharpness10, double sharpness20,
double gamma1, double gamma2) {
// Gauss-Legendre 20-point weights & nodes
const double gauss_weights[20] = {
0.1527533871, 0.1491729865, 0.1420961093, 0.1316886384, 0.1181945319,
0.1019301198, 0.0832767416, 0.0626720483, 0.0406014298, 0.0176140071,
0.0176140071, 0.0406014298, 0.0626720483, 0.0832767416, 0.1019301198,
0.1181945319, 0.1316886384, 0.1420961093, 0.1491729865, 0.1527533871
};
const double gauss_nodes[20] = {
-0.9931285992, -0.9639719273, -0.9122344283, -0.8391169718, -0.7463319065,
-0.6360536807, -0.5108670019, -0.3737060887, -0.2277858511, -0.0765265211,
0.0765265211, 0.2277858511, 0.3737060887, 0.5108670019, 0.6360536807,
0.7463319065, 0.8391169718, 0.9122344283, 0.9639719273, 0.9931285992
};
double sum_y = 0.0;
for (int i = 0; i < 20; i++) {
double weight = 0.5 * s * gauss_weights;
// Calculate the clothoid theta, kappa, sharpness at current s.
double sharpness10_i;
eq18_single_clothoid(0,sharpness10,gamma1,&sharpness10_i);
double sharpness20_i;
eq18_single_clothoid(0,sharpness20,gamma2,&sharpness20_i);
double kappa10_i;
eq19_single_clothoid(0,sharpness10_i,kappa10_i,gamma1,&kappa10_i);
double kappa20_i;
eq19_single_clothoid(0,sharpness20_i,kappa20_i,gamma2,&kappa20_i);
double theta10_i;
eq20_single_clothoid(0,sharpness10_i,kappa10_i,theta10,gamma1,&theta10_i);
double theta20_i;
eq20_single_clothoid(0,sharpness20_i,kappa20_i,theta20,gamma2,&theta20_i);
// Compute new coordinates using Gauss quadrature integration
double dy = cos(theta10_i) * sin(theta20_i) * weight;
sum_y += dy;
}
return y0 + sum_y - ye;
}
float eq14_gauss_z(double s,
double z0, double ze,
double theta10, double theta20,
double kappa10, double kappa20,
double sharpness10, double sharpness20,
double gamma1, double gamma2) {
// Gauss-Legendre 20-point weights & nodes
const double gauss_weights[20] = {
0.1527533871, 0.1491729865, 0.1420961093, 0.1316886384, 0.1181945319,
0.1019301198, 0.0832767416, 0.0626720483, 0.0406014298, 0.0176140071,
0.0176140071, 0.0406014298, 0.0626720483, 0.0832767416, 0.1019301198,
0.1181945319, 0.1316886384, 0.1420961093, 0.1491729865, 0.1527533871
};
const double gauss_nodes[20] = {
-0.9931285992, -0.9639719273, -0.9122344283, -0.8391169718, -0.7463319065,
-0.6360536807, -0.5108670019, -0.3737060887, -0.2277858511, -0.0765265211,
0.0765265211, 0.2277858511, 0.3737060887, 0.5108670019, 0.6360536807,
0.7463319065, 0.8391169718, 0.9122344283, 0.9639719273, 0.9931285992
};
double sum_z = 0.0;
for (int i = 0; i < 20; i++) {
double weight = 0.5 * s * gauss_weights;
// Calculate the clothoid theta, kappa, sharpness at current s.
// Calculate the clothoid theta, kappa, sharpness at current s.
double sharpness10_i;
eq18_single_clothoid(0,sharpness10,gamma1,&sharpness10_i);
double kappa10_i;
eq19_single_clothoid(0,sharpness10_i,kappa10_i,gamma1,&kappa10_i);
double theta10_i;
eq20_single_clothoid(0,sharpness10_i,kappa10_i,theta10,gamma1,&theta10_i);
// Compute new coordinates using Gauss quadrature integration
double dz = sin(theta10_i) * weight;
sum_z += dz;
}
return z0 + sum_z - ze;
}
int
func_f (const gsl_vector * x, void *params, gsl_vector * f)
{
struct model_params *par = (struct model_params *) params;
double g11 = gsl_vector_get(x, 0);
double g21 = gsl_vector_get(x, 1);
double s1 = gsl_vector_get(x, 2);
double f1 = eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double f2 = eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double f3 = eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
gsl_vector_set(f, 0, f1);
gsl_vector_set(f, 1, f2);
gsl_vector_set(f, 2, f3);
return GSL_SUCCESS;
}
int
func_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
struct model_params *par = (struct model_params *) params;
double g_delta = 0.001;
double s_delta = 0.001;
double g11 = gsl_vector_get(x, 0);
double dg11 = g11 + g_delta;
double g21 = gsl_vector_get(x, 1);
double dg21 = g21 + g_delta;
double s1 = gsl_vector_get(x, 2);
double ds1 = s1 + s_delta;
// Jacobi 1. row
gsl_matrix_set(J, 0, 0, (eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
dg11, g21) -
eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 0, 1, (eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, dg21) -
eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 0, 2, (eq14_gauss_x(ds1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21) -
eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / s_delta
);
// Jacobi 2. row
gsl_matrix_set(J, 1, 0, (eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
dg11, g21) -
eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 1, 1, (eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, dg21) -
eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 1, 2, (eq14_gauss_y(ds1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21) -
eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / s_delta
);
// Jacobi 3. row
gsl_matrix_set(J, 2, 0, (eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
dg11, g21) -
eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 2, 1, (eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, dg21) -
eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 2, 2, (eq14_gauss_z(ds1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21) -
eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / s_delta
);
return GSL_SUCCESS;
}
void
callback(const size_t iter, void *params,
const gsl_multifit_nlinear_workspace *w)
{
gsl_vector * x = gsl_multifit_nlinear_position(w);
double g11 = gsl_vector_get(x, 0);
double g21 = gsl_vector_get(x, 1);
double s1 = gsl_vector_get(x, 2);
/* print out current location */
printf("%f %f %f\n", g11, g21, s1);
}
void
solve_system(gsl_vector *x0, gsl_multifit_nlinear_fdf *fdf,
gsl_multifit_nlinear_parameters *params)
{
const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
const size_t max_iter = 200;
const double xtol = 1.0e-8;
const double gtol = 1.0e-8;
const double ftol = 1.0e-8;
const size_t n = fdf->n;
const size_t p = fdf->p;
gsl_multifit_nlinear_workspace *work =
gsl_multifit_nlinear_alloc(T, params, n, p);
gsl_vector * f = gsl_multifit_nlinear_residual(work);
gsl_vector * x = gsl_multifit_nlinear_position(work);
int info;
double chisq0, chisq, rcond;
printf("# %s/%s\n",
gsl_multifit_nlinear_name(work),
gsl_multifit_nlinear_trs_name(work));
/* initialize solver */
gsl_multifit_nlinear_init(x0, fdf, work);
/* store initial cost */
gsl_blas_ddot(f, f, &chisq0);
/* iterate until convergence */
gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
callback, NULL, &info, work);
/* store final cost */
gsl_blas_ddot(f, f, &chisq);
/* store cond(J(x)) */
gsl_multifit_nlinear_rcond(&rcond, work);
printf("\n");
struct model_params *par = (struct model_params *) params;
double g11 = gsl_vector_get(x, 0);
double g21 = gsl_vector_get(x, 1);
double s1 = gsl_vector_get(x, 2);
double x_res = eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double y_res = eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double z_res = eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
printf("Position Error: (%f %f %f)\n", x_res, y_res, z_res);
/* print summary */
fprintf(stderr, "%-25s %-6zu %-5zu %-5zu %-13.4e %-12.4e %-13.4e (%.2e, %.2e, %.2e)\n",
gsl_multifit_nlinear_trs_name(work),
gsl_multifit_nlinear_niter(work),
fdf->nevalf,
fdf->nevaldf,
chisq0,
chisq,
1.0 / rcond,
gsl_vector_get(x, 0),
gsl_vector_get(x, 1),
gsl_vector_get(x, 2));
printf("\n\n");
gsl_multifit_nlinear_free(work);
}
int
main (void)
{
const size_t n = 3;
const size_t p = 3;
gsl_vector *f = gsl_vector_alloc(n);
gsl_vector *x = gsl_vector_alloc(p);
gsl_multifit_nlinear_fdf fdf;
gsl_multifit_nlinear_parameters fdf_params =
gsl_multifit_nlinear_default_parameters();
struct model_params params;
params.t10 = 45.0 * (M_PI / 180.0);
params.t20 = 0.0 * (M_PI / 180.0);
params.k10 = 0.0;
params.k20 = 0.0;
params.c10 = 0.0;
params.c20 = 0.0;
params.xs = 0.0;
params.ys = 0.0;
params.zs = 0.0;
params.xe = 11.11;
params.ye = 22.22;
params.ze = 33.33;
params.xi;
params.yi;
params.zi;
/*
// print map of Phi(x1, x2)
{
double g11, g21, s1, chisq;
for (g11 = -5.0; g11 < 15.0; g11 += 0.1)
{
for (g21 = -5.0; g21 < 15.0; g21 += 0.1)
{
for (s1 = -5.0; s1 < 15.0; s1 += 0.1)
{
gsl_vector_set(x, 0, g11);
gsl_vector_set(x, 1, g21);
gsl_vector_set(x, 2, s1);
func_f(x, ¶ms, f);
gsl_blas_ddot(f, f, &chisq);
printf("%f %f %f %f\n", g11, g21, s1, chisq);
}
printf("\n");
}
printf("\n");
}
printf("\n\n");
}
/* define function to be minimized */
fdf.f = func_f;
fdf.df = func_df;
fdf.n = n;
fdf.p = p;
fdf.params = ¶ms;
/* starting point */
gsl_vector_set(x, 0, 0.01);
gsl_vector_set(x, 1, 0.01);
gsl_vector_set(x, 2, 0.01);
fprintf(stderr, "%-25s %-6s %-5s %-5s %-13s %-12s %-13s %-15s\n",
"Method", "NITER", "NFEV", "NJEV", "Initial Cost",
"Final cost", "Final cond(J)", "Final x");
fdf_params.trs = gsl_multifit_nlinear_trs_lm;
solve_system(x, &fdf, &fdf_params);
fdf_params.trs = gsl_multifit_nlinear_trs_dogleg;
solve_system(x, &fdf, &fdf_params);
fdf_params.trs = gsl_multifit_nlinear_trs_ddogleg;
solve_system(x, &fdf, &fdf_params);
fdf_params.trs = gsl_multifit_nlinear_trs_subspace2D;
solve_system(x, &fdf, &fdf_params);
gsl_vector_free(f);
gsl_vector_free(x);
return 0;
}
The idea was to calculate the elements of the Jacobian numerically but it fails miserably:
Warning: Spoiler!
#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlinear.h>
/* eq18, page 4.
*
* stot = total clothoid length
* s = global length at clothoid to retrieve sharpness c.
*
* Retrieve the sharpness for s.
*
*/
void eq18(const double s, const double stot,
const double sharpnessi0, const double sharpnessi1, const double sharpnessi2, const double sharpnessi3,
const double yi1, const double yi2, const double yi3, const double yi4,
double *sharpness_at_s) {
double s1 = stot / 4.0;
double s2 = s1 + s1;
double s3 = s1 + s1 + s1;
double s4 = stot;
if (s <= s1) {
*sharpness_at_s = sharpnessi0 + yi1 * s;
} else if (s > s1 && s <= s2) {
*sharpness_at_s = sharpnessi1 + yi2 * (s - s1);
} else if (s > s2 && s <= s3) {
*sharpness_at_s = sharpnessi2 + yi3 * (s - s2);
} else if (s > s3 && s <= s4) {
*sharpness_at_s = sharpnessi3 + yi4 * (s - s3);
} else {
// Handle the case where s is out of bounds (optional)
*sharpness_at_s = 0.0; // or some other default value
printf("error: eq18. \n");
}
}
void eq18_single_clothoid(const double s,
const double sharpnessi0,
const double yi1,
double *sharpness_at_s) {
*sharpness_at_s = sharpnessi0 + yi1 * s;
}
/* eq19, page 4.
*
* stot = total clothoid length
* s = global length at clothoid to retrieve sharpness c.
*
* Retrieve the kappa for s.
*
*/
void eq19(const double s, const double stot,
const double sharpnessi0, const double sharpnessi1, const double sharpnessi2, const double sharpnessi3,
const double kappai0, const double kappai1, const double kappai2, const double kappai3,
const double yi1, const double yi2, const double yi3, const double yi4,
double *kappa_at_s) {
double s1 = stot / 4.0;
double s2 = s1 + s1;
double s3 = s1 + s1 + s1;
double s4 = stot;
if (s <= s1) {
*kappa_at_s = kappai0 + sharpnessi0 * s + 0.5 * yi1 * (s * s);
} else if (s > s1 && s <= s2) {
*kappa_at_s = kappai1 + sharpnessi1 * (s - s1) + 0.5 * yi2 * ((s - s1) * (s - s1));
} else if (s > s2 && s <= s3) {
*kappa_at_s = kappai2 + sharpnessi2 * (s - s2) + 0.5 * yi3 * ((s - s2) * (s - s2));
} else if (s > s3 && s <= s4) {
*kappa_at_s = kappai3 + sharpnessi3 * (s - s3) + 0.5 * yi4 * ((s - s3) * (s - s3));
} else {
// Handle the case where s is out of bounds (optional)
*kappa_at_s = 0.0; // or some other default value
printf("error: eq19. \n");
}
}
void eq19_single_clothoid(const double s,
const double sharpnessi0,
const double kappai0,
const double yi1,
double *kappa_at_s) {
*kappa_at_s = kappai0 + sharpnessi0 * s + 0.5 * yi1 * (s * s);
}
/* eq20, page 4.
*
* stot = total clothoid length
* s = global length at clothoid to retrieve sharpness c.
*
* Retrieve the theta at s.
*
*/
void eq20(const double s, const double stot,
const double sharpnessi0, const double sharpnessi1, const double sharpnessi2, const double sharpnessi3,
const double kappai0, const double kappai1, const double kappai2, const double kappai3,
const double thetai0, const double thetai1, const double thetai2, const double thetai3,
const double yi1, const double yi2, const double yi3, const double yi4,
double *theta_at_s) {
double s1 = stot / 4.0;
double s2 = s1 + s1;
double s3 = s1 + s1 + s1;
double s4 = stot;
if (s <= s1) {
*theta_at_s = thetai0 + kappai0 * s + (0.5*sharpnessi0) * (s * s) + (1.0 / 6.0) * yi1 * (s * s * s);
} else if (s > s1 && s <= s2) {
*theta_at_s = thetai1 + kappai1 * (s - s1) + (0.5*sharpnessi1) * ((s - s1) * (s - s1)) + (1.0 / 6.0) * yi2 * ((s - s1) * (s - s1) * (s - s1));
} else if (s > s2 && s <= s3) {
*theta_at_s = thetai2 + kappai2 * (s - s2) + (0.5*sharpnessi2) * ((s - s2) * (s - s2)) + (1.0 / 6.0) * yi3 * ((s - s2) * (s - s2) * (s - s2));
} else if (s > s3 && s <= s4) {
*theta_at_s = thetai3 + kappai3 * (s - s3) + (0.5*sharpnessi3) * ((s - s3) * (s - s3)) + (1.0 / 6.0) * yi4 * ((s - s3) * (s - s3) * (s - s3));
} else {
// Handle the case where s is out of bounds (optional)
*theta_at_s = 0.0; // or some other default value
printf("error: eq20.\n");
}
}
/* Get the theta given s (length) for a single clothoid.
*
*/
void eq20_single_clothoid(const double s,
const double sharpnessi0,
const double kappai0,
const double thetai0,
const double yi1,
double *theta_at_s) {
*theta_at_s = thetai0 + kappai0 * s + (0.5*sharpnessi0) * (s * s) + (1.0 / 6.0) * yi1 * (s * s * s);
}
/* parameters to model */
struct model_params
{
double t10;
double t20;
double k10;
double k20;
double c10;
double c20;
double xs;
double ys;
double zs;
double xe;
double ye;
double ze;
double xi;
double yi;
double zi;
};
/* eq14, page 4
*
* Most efficient and precise solution to compute the clothoid curve lenght.
* Using gauss legendre, up to 20 points. Use a 30 point legendre to get even
* more accuracy.
*
* This calculation is more accurate then the above eq14_drift function.
*
* s = lenght on clothoid to retrieve the xyz point, xi,yi,zi.
* x0 y0 z0 = Clothoid start point.
* ltot = Clothoid length up to s.
*
* gamma1, gamma2, clothoid's rigid body factor. Synonims for y11, y22.
*
*/
float eq14_gauss_x(double s,
double x0, double xe,
double theta10, double theta20,
double kappa10, double kappa20,
double sharpness10, double sharpness20,
double gamma1, double gamma2) {
// Gauss-Legendre 20-point weights & nodes
const double gauss_weights[20] = {
0.1527533871, 0.1491729865, 0.1420961093, 0.1316886384, 0.1181945319,
0.1019301198, 0.0832767416, 0.0626720483, 0.0406014298, 0.0176140071,
0.0176140071, 0.0406014298, 0.0626720483, 0.0832767416, 0.1019301198,
0.1181945319, 0.1316886384, 0.1420961093, 0.1491729865, 0.1527533871
};
const double gauss_nodes[20] = {
-0.9931285992, -0.9639719273, -0.9122344283, -0.8391169718, -0.7463319065,
-0.6360536807, -0.5108670019, -0.3737060887, -0.2277858511, -0.0765265211,
0.0765265211, 0.2277858511, 0.3737060887, 0.5108670019, 0.6360536807,
0.7463319065, 0.8391169718, 0.9122344283, 0.9639719273, 0.9931285992
};
double sum_x = 0.0;
for (int i = 0; i < 20; i++) {
double weight = 0.5 * s * gauss_weights;
// Calculate the clothoid theta, kappa, sharpness at current s.
double sharpness10_i;
eq18_single_clothoid(0,sharpness10,gamma1,&sharpness10_i);
double sharpness20_i;
eq18_single_clothoid(0,sharpness20,gamma2,&sharpness20_i);
double kappa10_i;
eq19_single_clothoid(0,sharpness10_i,kappa10_i,gamma1,&kappa10_i);
double kappa20_i;
eq19_single_clothoid(0,sharpness20_i,kappa20_i,gamma2,&kappa20_i);
double theta10_i;
eq20_single_clothoid(0,sharpness10_i,kappa10_i,theta10,gamma1,&theta10_i);
double theta20_i;
eq20_single_clothoid(0,sharpness20_i,kappa20_i,theta20,gamma2,&theta20_i);
// Compute new coordinates using Gauss quadrature integration
double dx = cos(theta10_i) * cos(theta20_i) * weight;
sum_x += dx;
}
return x0 + sum_x - xe;
}
float eq14_gauss_y(double s,
double y0, double ye,
double theta10, double theta20,
double kappa10, double kappa20,
double sharpness10, double sharpness20,
double gamma1, double gamma2) {
// Gauss-Legendre 20-point weights & nodes
const double gauss_weights[20] = {
0.1527533871, 0.1491729865, 0.1420961093, 0.1316886384, 0.1181945319,
0.1019301198, 0.0832767416, 0.0626720483, 0.0406014298, 0.0176140071,
0.0176140071, 0.0406014298, 0.0626720483, 0.0832767416, 0.1019301198,
0.1181945319, 0.1316886384, 0.1420961093, 0.1491729865, 0.1527533871
};
const double gauss_nodes[20] = {
-0.9931285992, -0.9639719273, -0.9122344283, -0.8391169718, -0.7463319065,
-0.6360536807, -0.5108670019, -0.3737060887, -0.2277858511, -0.0765265211,
0.0765265211, 0.2277858511, 0.3737060887, 0.5108670019, 0.6360536807,
0.7463319065, 0.8391169718, 0.9122344283, 0.9639719273, 0.9931285992
};
double sum_y = 0.0;
for (int i = 0; i < 20; i++) {
double weight = 0.5 * s * gauss_weights;
// Calculate the clothoid theta, kappa, sharpness at current s.
double sharpness10_i;
eq18_single_clothoid(0,sharpness10,gamma1,&sharpness10_i);
double sharpness20_i;
eq18_single_clothoid(0,sharpness20,gamma2,&sharpness20_i);
double kappa10_i;
eq19_single_clothoid(0,sharpness10_i,kappa10_i,gamma1,&kappa10_i);
double kappa20_i;
eq19_single_clothoid(0,sharpness20_i,kappa20_i,gamma2,&kappa20_i);
double theta10_i;
eq20_single_clothoid(0,sharpness10_i,kappa10_i,theta10,gamma1,&theta10_i);
double theta20_i;
eq20_single_clothoid(0,sharpness20_i,kappa20_i,theta20,gamma2,&theta20_i);
// Compute new coordinates using Gauss quadrature integration
double dy = cos(theta10_i) * sin(theta20_i) * weight;
sum_y += dy;
}
return y0 + sum_y - ye;
}
float eq14_gauss_z(double s,
double z0, double ze,
double theta10, double theta20,
double kappa10, double kappa20,
double sharpness10, double sharpness20,
double gamma1, double gamma2) {
// Gauss-Legendre 20-point weights & nodes
const double gauss_weights[20] = {
0.1527533871, 0.1491729865, 0.1420961093, 0.1316886384, 0.1181945319,
0.1019301198, 0.0832767416, 0.0626720483, 0.0406014298, 0.0176140071,
0.0176140071, 0.0406014298, 0.0626720483, 0.0832767416, 0.1019301198,
0.1181945319, 0.1316886384, 0.1420961093, 0.1491729865, 0.1527533871
};
const double gauss_nodes[20] = {
-0.9931285992, -0.9639719273, -0.9122344283, -0.8391169718, -0.7463319065,
-0.6360536807, -0.5108670019, -0.3737060887, -0.2277858511, -0.0765265211,
0.0765265211, 0.2277858511, 0.3737060887, 0.5108670019, 0.6360536807,
0.7463319065, 0.8391169718, 0.9122344283, 0.9639719273, 0.9931285992
};
double sum_z = 0.0;
for (int i = 0; i < 20; i++) {
double weight = 0.5 * s * gauss_weights;
// Calculate the clothoid theta, kappa, sharpness at current s.
// Calculate the clothoid theta, kappa, sharpness at current s.
double sharpness10_i;
eq18_single_clothoid(0,sharpness10,gamma1,&sharpness10_i);
double kappa10_i;
eq19_single_clothoid(0,sharpness10_i,kappa10_i,gamma1,&kappa10_i);
double theta10_i;
eq20_single_clothoid(0,sharpness10_i,kappa10_i,theta10,gamma1,&theta10_i);
// Compute new coordinates using Gauss quadrature integration
double dz = sin(theta10_i) * weight;
sum_z += dz;
}
return z0 + sum_z - ze;
}
int
func_f (const gsl_vector * x, void *params, gsl_vector * f)
{
struct model_params *par = (struct model_params *) params;
double g11 = gsl_vector_get(x, 0);
double g21 = gsl_vector_get(x, 1);
double s1 = gsl_vector_get(x, 2);
double f1 = eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double f2 = eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double f3 = eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
gsl_vector_set(f, 0, f1);
gsl_vector_set(f, 1, f2);
gsl_vector_set(f, 2, f3);
return GSL_SUCCESS;
}
int
func_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
struct model_params *par = (struct model_params *) params;
double g_delta = 0.001;
double s_delta = 0.001;
double g11 = gsl_vector_get(x, 0);
double dg11 = g11 + g_delta;
double g21 = gsl_vector_get(x, 1);
double dg21 = g21 + g_delta;
double s1 = gsl_vector_get(x, 2);
double ds1 = s1 + s_delta;
// Jacobi 1. row
gsl_matrix_set(J, 0, 0, (eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
dg11, g21) -
eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 0, 1, (eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, dg21) -
eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 0, 2, (eq14_gauss_x(ds1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21) -
eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / s_delta
);
// Jacobi 2. row
gsl_matrix_set(J, 1, 0, (eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
dg11, g21) -
eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 1, 1, (eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, dg21) -
eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 1, 2, (eq14_gauss_y(ds1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21) -
eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / s_delta
);
// Jacobi 3. row
gsl_matrix_set(J, 2, 0, (eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
dg11, g21) -
eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 2, 1, (eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, dg21) -
eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / g_delta
);
gsl_matrix_set(J, 2, 2, (eq14_gauss_z(ds1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21) -
eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21)) / s_delta
);
return GSL_SUCCESS;
}
void
callback(const size_t iter, void *params,
const gsl_multifit_nlinear_workspace *w)
{
gsl_vector * x = gsl_multifit_nlinear_position(w);
double g11 = gsl_vector_get(x, 0);
double g21 = gsl_vector_get(x, 1);
double s1 = gsl_vector_get(x, 2);
/* print out current location */
printf("%f %f %f\n", g11, g21, s1);
}
void
solve_system(gsl_vector *x0, gsl_multifit_nlinear_fdf *fdf,
gsl_multifit_nlinear_parameters *params)
{
const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
const size_t max_iter = 200;
const double xtol = 1.0e-8;
const double gtol = 1.0e-8;
const double ftol = 1.0e-8;
const size_t n = fdf->n;
const size_t p = fdf->p;
gsl_multifit_nlinear_workspace *work =
gsl_multifit_nlinear_alloc(T, params, n, p);
gsl_vector * f = gsl_multifit_nlinear_residual(work);
gsl_vector * x = gsl_multifit_nlinear_position(work);
int info;
double chisq0, chisq, rcond;
printf("# %s/%s\n",
gsl_multifit_nlinear_name(work),
gsl_multifit_nlinear_trs_name(work));
/* initialize solver */
gsl_multifit_nlinear_init(x0, fdf, work);
/* store initial cost */
gsl_blas_ddot(f, f, &chisq0);
/* iterate until convergence */
gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
callback, NULL, &info, work);
/* store final cost */
gsl_blas_ddot(f, f, &chisq);
/* store cond(J(x)) */
gsl_multifit_nlinear_rcond(&rcond, work);
printf("\n");
struct model_params *par = (struct model_params *) params;
double g11 = gsl_vector_get(x, 0);
double g21 = gsl_vector_get(x, 1);
double s1 = gsl_vector_get(x, 2);
double x_res = eq14_gauss_x(s1,
par->xs, par->xe,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double y_res = eq14_gauss_y(s1,
par->ys, par->ye,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
double z_res = eq14_gauss_z(s1,
par->zs, par->ze,
par->t10, par->t20,
par->k10, par->k20,
par->c10, par->c20,
g11, g21);
printf("Position Error: (%f %f %f)\n", x_res, y_res, z_res);
/* print summary */
fprintf(stderr, "%-25s %-6zu %-5zu %-5zu %-13.4e %-12.4e %-13.4e (%.2e, %.2e, %.2e)\n",
gsl_multifit_nlinear_trs_name(work),
gsl_multifit_nlinear_niter(work),
fdf->nevalf,
fdf->nevaldf,
chisq0,
chisq,
1.0 / rcond,
gsl_vector_get(x, 0),
gsl_vector_get(x, 1),
gsl_vector_get(x, 2));
printf("\n\n");
gsl_multifit_nlinear_free(work);
}
int
main (void)
{
const size_t n = 3;
const size_t p = 3;
gsl_vector *f = gsl_vector_alloc(n);
gsl_vector *x = gsl_vector_alloc(p);
gsl_multifit_nlinear_fdf fdf;
gsl_multifit_nlinear_parameters fdf_params =
gsl_multifit_nlinear_default_parameters();
struct model_params params;
params.t10 = 45.0 * (M_PI / 180.0);
params.t20 = 0.0 * (M_PI / 180.0);
params.k10 = 0.0;
params.k20 = 0.0;
params.c10 = 0.0;
params.c20 = 0.0;
params.xs = 0.0;
params.ys = 0.0;
params.zs = 0.0;
params.xe = 11.11;
params.ye = 22.22;
params.ze = 33.33;
params.xi;
params.yi;
params.zi;
/*
// print map of Phi(x1, x2)
{
double g11, g21, s1, chisq;
for (g11 = -5.0; g11 < 15.0; g11 += 0.1)
{
for (g21 = -5.0; g21 < 15.0; g21 += 0.1)
{
for (s1 = -5.0; s1 < 15.0; s1 += 0.1)
{
gsl_vector_set(x, 0, g11);
gsl_vector_set(x, 1, g21);
gsl_vector_set(x, 2, s1);
func_f(x, ¶ms, f);
gsl_blas_ddot(f, f, &chisq);
printf("%f %f %f %f\n", g11, g21, s1, chisq);
}
printf("\n");
}
printf("\n");
}
printf("\n\n");
}
/* define function to be minimized */
fdf.f = func_f;
fdf.df = func_df;
fdf.n = n;
fdf.p = p;
fdf.params = ¶ms;
/* starting point */
gsl_vector_set(x, 0, 0.01);
gsl_vector_set(x, 1, 0.01);
gsl_vector_set(x, 2, 0.01);
fprintf(stderr, "%-25s %-6s %-5s %-5s %-13s %-12s %-13s %-15s\n",
"Method", "NITER", "NFEV", "NJEV", "Initial Cost",
"Final cost", "Final cond(J)", "Final x");
fdf_params.trs = gsl_multifit_nlinear_trs_lm;
solve_system(x, &fdf, &fdf_params);
fdf_params.trs = gsl_multifit_nlinear_trs_dogleg;
solve_system(x, &fdf, &fdf_params);
fdf_params.trs = gsl_multifit_nlinear_trs_ddogleg;
solve_system(x, &fdf, &fdf_params);
fdf_params.trs = gsl_multifit_nlinear_trs_subspace2D;
solve_system(x, &fdf, &fdf_params);
gsl_vector_free(f);
gsl_vector_free(x);
return 0;
}
The idea was to calculate the elements of the Jacobian numerically but it fails miserably:
Time to create page: 0.470 seconds