main.cpp

00001 // IMU for Sparkfun 9DOF Sensor Stick
00002 
00003 #include "mbed.h"
00004 #include "ADXL345_I2C.h"
00005 #include "ITG3200.h"
00006 #include "HMC5883L.h"
00007 #include "TextLCD.h"
00008 
00009 DigitalOut led1(LED1);
00010 DigitalOut led2(LED2);
00011 DigitalOut led3(LED3);
00012 DigitalOut led4(LED4);
00013 ADXL345_I2C accelerometer(p9, p10);
00014 ITG3200 gyro(p9, p10);
00015 HMC5883L compass(p9, p10);
00016 TextLCD lcd(p24, p26, p27, p28, p29, p30);
00017 Serial pc(USBTX, USBRX);
00018 
00019 #define pi       3.14159265
00020 
00021 #define N        3
00022 #define N_LWMA   10
00023 
00024 double* RK4( double, double[N], double[N] );
00025 double* func( double[N], double[N] );
00026 double* LWMA( double[N] );
00027 double* EKF_predict( double[N], double[N] );
00028 double* EKF_correct( double[N], double[N], double[N] );
00029 
00030 //             0       1       2
00031 //        [ accXn-1 accXn-2   ...   ] 0
00032 // zBuf = [ accYn-1 accYn-2   ...   ] 1
00033 //        [ accZn-1 accZn-2   ...   ] 2
00034 double z_buf[N][N_LWMA]    =    {0};                // For LWMA
00035 
00036 double P[N][N]    =    {{1,0,0},{0,1,0},{0,0,1}};    // For EKF
00037 
00038 int main(){ 
00039     
00040     pc.baud(921600);
00041     
00042     // loop
00043     int correct_loop    =   0;
00044     int calib_loop  =   1000;
00045     int lcd_loop    =   0;
00046     
00047     //double dt_wait = 0.00514;
00048     double dt_wait = 0.0046;
00049     double dt =   0.01;
00050     double t  = 0;    
00051 
00052     int bit_acc[N]   =   {0};    // Buffer of the accelerometer
00053     int get_mag[N]   =   {0};    // Buffer of the compass
00054     
00055     // Calibration routine
00056     double calib_acc[N]         =   {0};
00057     double calib_gyro_buf[N]    =   {0};
00058     double calib_gyro[N]        =   {0};
00059     double compass_basis_buf[N] =   {0};
00060     double compass_basis_rad    =   0;
00061     
00062     // Getting data
00063     double acc[N]       =   {0};
00064     double gyro_deg[N]  =   {0};
00065     double gyro_rad[N]  =   {0};
00066     int    mag[N]       =   {0};
00067     
00068     // Measurement algorithm
00069     double ang_acc[2]   =   {0};
00070     double ang_deg[N]   =   {0};
00071     double ang_rad[N]   =   {0};
00072     double zLWMA[N]     =   {0};
00073     double compass_rad  =   0;
00074     double compass_deg  =   0;
00075     
00076     for( int i=0;i<N_LWMA;i++ ){
00077         z_buf[2][i]    =    1;
00078     }
00079     
00080     double* p_ang_RK4;
00081     double* p_ang_EKF;
00082     double* p_zLWMA_buf;
00083     
00084     // *** Setting up accelerometer ***
00085     // These are here to test whether any of the initialization fails. It will print the failure.
00086     if(accelerometer.setPowerControl(0x00)){
00087         pc.printf("didn't intitialize power control\n\r"); 
00088         return 0;
00089     }
00090     // Full resolution, +/-16g, 4mg/LSB.
00091     wait(.001);
00092      
00093     if(accelerometer.setDataFormatControl(0x0B)){
00094         pc.printf("didn't set data format\n\r");
00095         return 0;  }
00096     wait(.001);
00097      
00098     // 3.2kHz data rate.
00099     if(accelerometer.setDataRate(ADXL345_3200HZ)){
00100         pc.printf("didn't set data rate\n\r");
00101         return 0;
00102     }
00103     wait(.001);
00104       
00105     if(accelerometer.setPowerControl(MeasurementMode)) {
00106         pc.printf("didn't set the power control to measurement\n\r"); 
00107         return 0;
00108     } 
00109     // *** Setting up accelerometer ***
00110     
00111     // *** Setting up gyro ***
00112     gyro.setLpBandwidth(LPFBW_42HZ);
00113 
00114     // *** Setting up compass ***
00115     compass.setDefault();
00116     
00117     // Wait some time for all sensors (Need at least 5ms)
00118     wait(0.1);
00119     
00120     // *** Calibration routine ***
00121     pc.printf("\n\rDon't touch! Calibrating now!\n\r");
00122     lcd.locate(0, 0);   lcd.printf("Don't touch!");
00123     lcd.locate(0, 1);   lcd.printf("Calibrating now!");
00124     led1    =   1;
00125     for( int i=0;i<calib_loop;i++ ){
00126         accelerometer.getOutput(bit_acc);
00127         compass.readData(get_mag);
00128         calib_gyro_buf[0]   =   gyro.getGyroX();
00129         calib_gyro_buf[1]   =   gyro.getGyroY();
00130         calib_gyro_buf[2]   =   gyro.getGyroZ();
00131         for( int j=0;j<N;j++ ){
00132             calib_acc[j]            +=  (int16_t)bit_acc[j];
00133             calib_gyro[j]           +=  calib_gyro_buf[j];
00134             compass_basis_buf[j]    +=  (int16_t)get_mag[j];
00135         }
00136         if( i>calib_loop*3/4 ){
00137              led4   =   1;
00138         }else if( i>calib_loop/2 ){
00139             led3    =   1;
00140         }else if( i>calib_loop/4 ){
00141             led2    =   1;
00142         }
00143     }
00144     for( int i=0;i<N;i++ ){
00145         calib_acc[i]            =   calib_acc[i]/calib_loop;
00146         calib_gyro[i]           =   calib_gyro[i]/calib_loop;
00147         compass_basis_buf[i]    =   compass_basis_buf[i]/calib_loop;
00148     }
00149     compass_basis_rad   =   compass_basis_buf[1]/compass_basis_buf[0];
00150     compass_basis_rad   =   atan(compass_basis_rad);
00151     led1 = 0;    led2 = 0;  led3 = 0;   led4    =   0;
00152     pc.printf("accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
00153     pc.printf("%f, %f, %f, %f, %f, %f, %f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
00154     lcd.cls();
00155     lcd.locate(0, 0);   lcd.printf("Starting IMU");
00156     lcd.locate(0, 1);   lcd.printf("IMU with EKF");
00157     for( int i=0;i<3;i++ ){
00158         wait(0.5);
00159         led1 = 1;    led2 = 1;  led3 = 1;   led4    =   1;
00160         wait(0.5);
00161         led1 = 0;    led2 = 0;  led3 = 0;   led4    =   0;
00162     }
00163     // *** Calibration routine ***
00164     
00165     lcd.cls();
00166     pc.printf("Starting IMU unit \n\r");
00167     pc.printf("   Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n\r");
00168     while( 1 ){
00169     //for( int i=0;i<10000;i++ ){
00170         
00171         // Updating accelerometer and compass
00172         accelerometer.getOutput(bit_acc);
00173         compass.readData(get_mag);
00174         
00175         // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
00176         acc[0]      =  -((int16_t)bit_acc[0]-calib_acc[0])*0.004;
00177         acc[1]      =  ((int16_t)bit_acc[1]-calib_acc[1])*0.004;
00178         acc[2]      =  ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
00179         
00180         gyro_deg[0] = (gyro.getGyroX()-calib_gyro[0])/14.375;
00181         gyro_deg[1] = (gyro.getGyroY()-calib_gyro[1])/14.375;
00182         gyro_deg[2] = (gyro.getGyroZ()-calib_gyro[2])/14.375;
00183         
00184         for( int i=0;i<N;i++ ){     mag[0] = (int16_t)get_mag[0];   }
00185         
00186         // Low pass filter for acc
00187         //for( int i=0;i<N;i++ ){    if( -0.05<acc[i] && acc[i]<0.05 ){    acc[i] = 0;  }  }
00188         
00189         for( int i=0;i<N;i++ ){     if( acc[i]<-2 ){    acc[i] = -2;    }  }
00190         for( int i=0;i<N;i++ ){     if( acc[i]>2 ){    acc[i] = 2;    }  }
00191         
00192         // Low pass filter for gyro
00193         //for( int i=0;i<N;i++ ){    if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){    gyro_deg[i] = 0;  } }
00194         
00195         for( int i=0;i<N;i++ ){    gyro_rad[i] = gyro_deg[i]*pi/180;    }
00196         
00197         // Compass yaw
00198         /*compass_rad    =    (double)mag[1]/mag[0];
00199         compass_rad    =    atan(compass_rad);
00200         //compass_rad    =    compass_rad-compass_basis_rad;
00201         compass_deg    =    compass_rad*180/pi;
00202         */
00203         // LWMA (Observation)
00204         p_zLWMA_buf = LWMA(acc);
00205         for( int i=0;i<N;i++ ){     zLWMA[i] = *p_zLWMA_buf;     p_zLWMA_buf = p_zLWMA_buf+1;    }
00206         // LWMA angle
00207         ang_acc[0] = asin(zLWMA[1])*180/pi;
00208         ang_acc[1] = asin(zLWMA[0])*180/pi;
00209         
00210         // RK4 (Prediction)
00211         p_ang_RK4 = RK4(dt,ang_rad,gyro_rad);
00212         for( int i=0;i<N;i++ ){    ang_rad[i] = *p_ang_RK4;        p_ang_RK4 = p_ang_RK4+1;    }
00213         
00214         // EKF (Correction)
00215         EKF_predict(ang_rad,gyro_rad);
00216         correct_loop++;
00217         if ( correct_loop>=10 ){
00218             p_ang_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
00219             for ( int i=0;i<N;i++ ){    ang_deg[i] = *p_ang_EKF;        p_ang_EKF = p_ang_EKF+1;    }
00220             correct_loop    =    0;
00221         }
00222         for( int i=0;i<N;i++ ){    ang_deg[i] = 1.76*ang_rad[i]*180/pi;    }
00223         
00224         //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
00225         //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2]);
00226         //pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
00227         //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2]);
00228         //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, zLWMA[0], zLWMA[1], zLWMA[2], ang_acc[0], ang_acc[1]);
00229         //pc.printf("%d, %d, %f\n\r",mag[0], mag[1], compass_deg);
00230         //pc.printf("%7.2f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, ang_acc[0], ang_acc[1], ang_deg[0], ang_deg[1], ang_deg[2]);
00231         pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
00232         //pc.printf("%7.2f, %7.1f, %7.1f,     0.0\n\r", t, ang_deg[0], ang_deg[1]);
00233         lcd_loop++;
00234         if( lcd_loop>=20 ){
00235             lcd.locate(0, 0);   lcd.printf("phi:%6.1f [deg]", ang_deg[0]);
00236             lcd.locate(0, 1);   lcd.printf("the:%6.1f [deg]", ang_deg[1]);
00237             lcd_loop    =   0;
00238         }
00239         
00240         wait(dt_wait);
00241         t += dt;
00242         
00243     }
00244     
00245      led1 = 1;    led2 = 1;  led3 = 1;   led4    =   1;
00246     
00247 }
00248 
00249 
00250 double* EKF_predict( double y[N], double x[N] ){ 
00251     // x = F * x;  
00252     // P = F * P * F' + G * Q * G';
00253 
00254     //double Q[N][N]        =    { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
00255     double Q[N][N]        =    { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
00256 
00257     double Fjac[N][N]    =    {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}};
00258     double Fjac_t[N][N]    =    {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}};
00259     double Gjac[N][N]    =    {{1, sin(y[0])*tan(y[1]), cos(y[0])*tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}};
00260     double Gjac_t[N][N]    =    {{1, 0, 0}, {sin(y[0])*tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])*tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}};
00261 
00262     double FPF[N][N]    =    {0};
00263     double GQG[N][N]    =    {0};
00264 
00265     double FP[N][N]        =    {0};
00266     double GQ[N][N]        =    {0};
00267     
00268     for( int i=0;i<N;i++ ){
00269         for( int j=0;j<N;j++ ){
00270             for( int k=0;k<N;k++ ){
00271                 FP[i][j]    +=    Fjac[i][k]*P[k][j];
00272                 GQ[i][j]    +=    Gjac[i][k]*Q[k][j];
00273 
00274             }
00275         }
00276     }
00277 
00278     for( int i=0;i<N;i++ ){
00279         for( int j=0;j<N;j++ ){
00280             for( int k=0;k<N;k++ ){
00281                 FPF[i][j]        +=    FP[i][k]*Fjac_t[k][j];
00282                 GQG[i][j]        +=    GQ[i][k]*Gjac_t[k][j];
00283             }
00284         }
00285     }
00286     for( int i=0;i<N;i++ ){
00287         for( int j=0;j<N;j++ ){
00288             P[i][j]        =    FPF[i][j]+GQG[i][j];
00289         }
00290     }
00291     
00292     return 0;
00293 
00294 }
00295 
00296 double* EKF_correct( double y[N], double x[N], double z[N] ){
00297     // K = P * H' / ( H * P * H' + R )
00298     // x = x + K * ( yobs(t) - H * x )
00299     // P = P - K * H * P
00300 
00301     //double R[N][N]        =    { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
00302     double R[N][N]        =    { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
00303 
00304     double Hjac[N][N]    =    {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}};
00305     double Hjac_t[N][N]    =    {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}};
00306     double K[N][N]        =    {0};    // Kalman gain
00307     double K_deno[N][N]    =    {0};    // Denominator of the kalman gain
00308     double det_K_deno_inv    =    0;
00309     double K_deno_inv[N][N]    =    {0};
00310     double HPH[N][N]    =    {0};
00311     double HP[N][N]        =    {0};
00312     double PH[N][N]        =    {0};
00313     double KHP[N][N]    =    {0};
00314 
00315     double Hx[N]        =    {0};
00316     double z_Hx[N]        =    {0};
00317     double Kz_Hx[N]        =    {0};
00318 
00319     double* py    =    y;
00320 
00321     // Kalman gain
00322     for( int i=0;i<N;i++ ){
00323         for( int j=0;j<N;j++ ){
00324             for( int k=0;k<N;k++ ){
00325                 HP[i][j]    +=    Hjac[i][k]*P[k][j];
00326                 PH[i][j]    +=    P[i][k]*Hjac_t[k][j];
00327             }
00328         }
00329     }
00330     for( int i=0;i<N;i++ ){
00331         for( int j=0;j<N;j++ ){
00332             for( int k=0;k<N;k++ ){
00333                 HPH[i][j]        +=    HP[i][k]*Hjac_t[k][j];
00334             }
00335         }
00336     }
00337     for( int i=0;i<N;i++ ){
00338         for( int j=0;j<N;j++ ){
00339             K_deno[i][j]    =    HPH[i][j]+R[i][j];
00340         }
00341     }
00342     // Inverce matrix
00343     det_K_deno_inv      =    K_deno[0][0]*K_deno[1][1]*K_deno[2][2]+K_deno[1][0]*K_deno[2][1]*K_deno[0][2]+K_deno[2][0]*K_deno[0][1]*K_deno[1][2]-K_deno[0][0]*K_deno[2][1]*K_deno[1][2]-K_deno[2][0]*K_deno[1][1]*K_deno[0][2]-K_deno[1][0]*K_deno[0][1]*K_deno[2][2];
00344     K_deno_inv[0][0]    =    (K_deno[1][1]*K_deno[2][2]-K_deno[1][2]*K_deno[2][1])/det_K_deno_inv;
00345     K_deno_inv[0][1]    =    (K_deno[0][2]*K_deno[2][1]-K_deno[0][1]*K_deno[2][2])/det_K_deno_inv;
00346     K_deno_inv[0][2]    =    (K_deno[0][1]*K_deno[1][2]-K_deno[0][2]*K_deno[1][1])/det_K_deno_inv;
00347     K_deno_inv[1][0]    =    (K_deno[1][2]*K_deno[2][0]-K_deno[1][0]*K_deno[2][2])/det_K_deno_inv;
00348     K_deno_inv[1][1]    =    (K_deno[0][0]*K_deno[2][2]-K_deno[0][2]*K_deno[2][0])/det_K_deno_inv;
00349     K_deno_inv[1][2]    =    (K_deno[0][2]*K_deno[1][0]-K_deno[0][0]*K_deno[1][2])/det_K_deno_inv;
00350     K_deno_inv[2][0]    =    (K_deno[1][0]*K_deno[2][1]-K_deno[1][1]*K_deno[2][0])/det_K_deno_inv;
00351     K_deno_inv[2][1]    =    (K_deno[0][1]*K_deno[2][0]-K_deno[0][0]*K_deno[2][1])/det_K_deno_inv;
00352     K_deno_inv[2][2]    =    (K_deno[0][0]*K_deno[1][1]-K_deno[0][1]*K_deno[1][0])/det_K_deno_inv;
00353 
00354     for( int i=0;i<N;i++ ){
00355         for( int j=0;j<N;j++ ){
00356             for( int k=0;k<N;k++ ){
00357                 K[i][j]        +=    PH[i][k]*K_deno_inv[k][j];
00358             }
00359         }
00360     }
00361 
00362     // Filtering
00363     for( int i=0;i<N;i++ ){
00364         for( int j=0;j<N;j++ ){
00365             Hx[i]        +=    Hjac[i][j]*y[j];
00366         }
00367     }
00368     for( int i=0;i<N;i++ ){
00369         z_Hx[i]    =    z[i]-Hx[i];
00370     }
00371     for( int i=0;i<N;i++ ){
00372         for( int j=0;j<N;j++ ){
00373             Kz_Hx[i]    +=    K[i][j]*z_Hx[j];
00374         }
00375     }
00376     for( int i=0;i<N;i++ ){
00377             y[i]    =    y[i]+Kz_Hx[i];
00378     }
00379 
00380     // Covarience
00381     for( int i=0;i<N;i++ ){
00382         for( int j=0;j<N;j++ ){
00383             for( int k=0;k<N;k++ ){
00384                 KHP[i][j]    +=    K[i][k]*HP[k][j];
00385             }
00386         }
00387     }
00388     for( int i=0;i<N;i++ ){
00389         for( int j=0;j<N;j++ ){
00390             P[i][j]    =    P[i][j]-KHP[i][j];
00391         }
00392     }
00393 
00394     return py;
00395 
00396 }
00397 
00398 double* LWMA( double z[N] ){
00399     
00400     double zLWMA[N]        =    {0};
00401     double zLWMA_num[N]    =    {0};
00402     double zLWMA_den    =    0;
00403 
00404     double* pzLWMA    =    zLWMA;
00405 
00406     for( int i=1;i<N_LWMA;i++ ){
00407         for( int j=0;j<N;j++ ){
00408             z_buf[j][N_LWMA-i]    =    z_buf[j][N_LWMA-i-1];
00409         }
00410     }
00411     for( int i=0;i<N;i++ ){
00412         z_buf[i][0]    =    z[i];
00413     }
00414 
00415     for( int i=0;i<N_LWMA;i++ ){
00416         for( int j=0;j<N;j++ ){
00417             zLWMA_num[j]    +=    (N_LWMA-i)*z_buf[j][i];    
00418         }
00419         zLWMA_den    +=    N_LWMA-i;
00420     }
00421     for( int i=0;i<N;i++ ){
00422         zLWMA[i]    =    zLWMA_num[i]/zLWMA_den;
00423     }
00424     
00425     return pzLWMA;
00426 
00427 }
00428 
00429 double* RK4( double dt, double y[N], double x[N] ){
00430     
00431     double yBuf[N]    =    {0};
00432     double k[N][4]    =    {0};
00433 
00434     double* p_y    =    y;
00435 
00436     double* pk1;
00437     double* pk2;
00438     double* pk3;
00439     double* pk4;
00440 
00441         for( int i=0;i<N;i++){    yBuf[i]    = y[i];    }
00442         pk1    =    func (yBuf,x);
00443         for( int i=0;i<N;i++ ){    k[i][0] = *pk1;        pk1    = pk1+1;    }
00444 
00445         for( int i=0;i<N;i++){    yBuf[i]    = y[i]+0.5*dt*k[i][1];    }
00446         pk2    =    func (yBuf,x);
00447         for( int i=0;i<N;i++ ){    k[i][1]    = *pk2;        pk2    = pk2+1;    }
00448 
00449         for( int i=0;i<N;i++){    yBuf[i]    = y[i]+0.5*dt*k[i][2];    }
00450         pk3    =    func (yBuf,x);
00451         for( int i=0;i<N;i++ ){    k[i][2]    = *pk3;        pk3    = pk3+1;    }
00452 
00453         for( int i=0;i<N;i++){    yBuf[i]    = y[i]+dt*k[i][3];    }
00454         pk4    =    func (yBuf,x);
00455         for( int i=0;i<N;i++ ){    k[i][3]    = *pk4;        pk4 = pk4+1;    }
00456 
00457         for( int i=0;i<N;i++){    y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0;    }
00458 
00459     return p_y;
00460 
00461 }
00462 
00463 double* func( double y[N], double x[N] ){
00464 
00465     double f[N]    =    {0};
00466     double* p_f    =    f;
00467     
00468     f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
00469     f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
00470     f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
00471 
00472     return p_f;
00473 
00474 }