C++で自然スプライン補完

C++で自然スプライン補完

#include<stdio.h>
#include<gsl/gsl_linalg.h>


void f(double* x,int size,double* x_knots,int knots_size,double* a,double* b,double* c,double* d,double* output){


        //x should be sorted;

        int j=0;
        double r3;
        double r2;
        double r1;
        double r0;
        int k=0;
        for(int i=0;i<size;i++){
                while(x[i]>=x_knots[k]){
                        k=k+1;

                }

                if(k>knots_size){
                        k=knots_size-1;
                }



                r3=a[k-1]*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1]);
                r2=b[k-1]*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1]);
                r1=c[k-1]*(x[i]-x_knots[k-1]);
                r0=d[k-1];

                output[i]=r3+r2+r1+r0;

        }


}


void ff(double* x,int size,double* x_knots,int knots_size,double* a,double* b,double* c,double* d,double* output){


        //x should be sorted;

        int j=0;
        double r3;
        double r2;
        double r1;
        double r0;
        int k=0;
        for(int i=0;i<size;i++){
                while(x[i]>=x_knots[k]){
                        k=k+1;

                }

                if(k>knots_size){
                        k=knots_size-1;
                }



        //      r3=a[k-1]*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1]);
        //      r2=b[k-1]*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1]);
        //      r1=c[k-1]*(x[i]-x_knots[k-1]);
        //      r0=d[k-1];

                output[i]=6*a[k-1]*(x[i]-x_knots[k-1])+2*b[k-1];


        }
}

void solve(double** m,double* v,int len,double* output){
        using namespace std;
        int s;
        double *mb;
        int k=0;
        mb=(double*)malloc(sizeof(double)*len*len);
        for(int i=0;i<len;i++){
                for(int j=0;j<len;j++){
//                      printf("%lf\n",m[i][j]);
                        mb[k]=m[i][j];
                        k++;

                }

        }


        gsl_matrix_view m2=gsl_matrix_view_array(mb,len,len);
        gsl_vector_view v2=gsl_vector_view_array(v,len);
        gsl_vector *x=gsl_vector_alloc(len);
        gsl_permutation *p =gsl_permutation_alloc(len);
        gsl_linalg_LU_decomp(&m2.matrix,p,&s);
        gsl_linalg_LU_solve(&m2.matrix,p,&v2.vector,x);
        printf("x=\n");

        gsl_vector_fprintf(stdout,x,"%g");
        gsl_permutation_free(p);
        //gsl_vector_free(x);
        for(int i=0;i<len;i++){
                output[i]=gsl_vector_get(x,i);

        };
//      return *v;

}


int main(){
        double* ytmp;
        double* x_knots;
        double* y_knots;
        double* h;
        double* v;
        double* u;

        double* a;
        double* b;
        double* c;
        double* d;
        double** m;
        int time_length=10;
        int len;
        len=time_length-1;

        ytmp=(double*) malloc(sizeof(double)*time_length);

        a=(double*) malloc(sizeof(double)*time_length);
        b=(double*) malloc(sizeof(double)*time_length);
        c=(double*) malloc(sizeof(double)*time_length);
        d=(double*) malloc(sizeof(double)*time_length);
        x_knots=(double*) malloc(sizeof(double)*time_length);
        y_knots=(double*) malloc(sizeof(double)*time_length);

        h=(double*) malloc(sizeof(double)*(time_length-1));

        v=(double*) malloc(sizeof(double)*(time_length-1));

        m=(double**) malloc(sizeof(double*)*(time_length-1));

        u=(double*) malloc(sizeof(double)*(time_length-2));
        for(int i=0;i<time_length-2;i++){
                m[i]=(double*)malloc(sizeof(double)*(time_length-1));

        }


//Inititalization
        for(int i=0;i<time_length;i++){

                //q[i]=i;
        }


        for(int i=0;i<time_length;i++){

                x_knots[i]=i;
        }

        for(int i=0;i<time_length;i++){

                y_knots[i]=i;

                //ytmp[i]=i;
        }

        ytmp[0]=5;
        ytmp[1]=2;
        ytmp[2]=3;
        ytmp[3]=1;
        ytmp[4]=4;
        ytmp[5]=2;
        ytmp[6]=3;
        ytmp[7]=1;
        ytmp[8]=8;
        ytmp[9]=9;

//Solve a natural spline

        for(int i=0;i<(time_length-1);i++){
                h[i]=x_knots[i+1]-x_knots[i];
        }

        for(int i=0;i<(time_length-2);i++){
                v[i]=6*((ytmp[i+2]-ytmp[i+1])/h[i+1]-(ytmp[i+1]-ytmp[i])/h[i]);
                printf("%lf\n",v[i]);

        }

        for(int i=0;i<(time_length-2);i++){
                for(int j=0;j<(time_length-2);j++){
                        m[i][j]=0;

                }
        }


        m[0][0]=2*(h[0]+h[1]);
        m[0][1]=h[1];

        for(int i=1;i<(len-2);i++){
                m[i][i-1]=h[i];
                m[i][i]=2*(h[i]+h[i+1]);
                m[i][i+1]=h[i+2];

        }

        m[len-2][len-3]=h[len-3];
        m[len-2][len-2]=2*(h[len-3]+h[len-2]);


        for(int i=0;i<(time_length-2);i++){
                for(int j=0;j<(time_length-2);j++){
                //      m[i][j]=0;
                        printf("%lf,",m[i][j]);

                }
                        printf("\n");
        }

        solve(m,v,time_length-2,u);

        b[0]=0;
        a[0]=(u[0]-0)/6.0/(x_knots[2]-x_knots[1]);
        c[0]=(ytmp[1]-ytmp[0])/(x_knots[2]-x_knots[1])-1.0/6.0*(x_knots[2]-x_knots[1])*(+u[0]);
        d[0]=ytmp[0];
        for(int i=1;i<time_length-1;i++){
                b[i]=u[i-1]/2.0;
                a[i]=(u[i]-u[i-1])/6.0/(x_knots[i+1]-x_knots[i]);
                d[i]=ytmp[i];
                c[i]=(ytmp[i+1]-ytmp[i])/(x_knots[i+1]-x_knots[i])-1.0/6.0*(x_knots[i+1]-x_knots[i])*(2*u[i-1]+u[i]);

        }

        d[time_length-1]=ytmp[time_length-1];
        double x[]={0.5,5.3,8.5,9.5};
        double q[10];
        double q2[10];
        f(x,4,x_knots,time_length,a,b,c,d,q);

        ff(x,4,x_knots,time_length,a,b,c,d,q2);
        printf("hello\n");
        for(int i=0;i<4;i++){
                printf("q:%lf\n",q[i]);
                printf("q2:%lf\n",q2[i]);

        }



        return 0;
}

平滑化スプライン、途中。

#include<stdio.h>
#include<gsl/gsl_linalg.h>
#include<math.h>
#include<gsl/gsl_randist.h>

void f(double* x,int size,double* x_knots,int knots_size,double* a,double* b,double* c,double* d,double* output){


        //x should be sorted;

        int j=0;
        double r3;
        double r2;
        double r1;
        double r0;
        int k=0;
        for(int i=0;i<size;i++){
                while(x[i]>=x_knots[k]){
                        k=k+1;

                }

                if(k>knots_size){
                        k=knots_size-1;
                }



                r3=a[k-1]*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1]);
                r2=b[k-1]*(x[i]-x_knots[k-1])*(x[i]-x_knots[k-1]);
                r1=c[k-1]*(x[i]-x_knots[k-1]);
                r0=d[k-1];

                output[i]=r3+r2+r1+r0;

        }


}


void ff(double*x, int size,double* x_knots,int knots_size,double* a,double* b,double* c,double* d,double* output){


        //x should be sorted;

        int j=0;
        double r3;
        double r2;
        double r1;
        double r0;
        int k=0;
        for(int i=0;i<size;i++){
                while(x[i]>=x_knots[k]){
                        k=k+1;

                }

                if(k>knots_size){
                        k=knots_size-1;
                }


                output[i]=6*a[k-1]*(x[i]-x_knots[k-1])+2*b[k-1];


        }
}

double ff_int(double* x_knots,int knots_size,double* a,double* b,double* c,double* d){

        double A,B,C,D,intint1,intint2;
        //x should be sorted;
        double sum2=0;
        for(int i=0;i<knots_size-1;i++){
                A=36*a[i]*a[i];
                B=-36*a[i]*a[i]*2*x_knots[i]+24*a[i]*b[i];
                C=-24*a[i]*b[i]*x_knots[i]+4*b[i]*b[i]+36*a[i]*a[i]*x_knots[i]*x_knots[i];

                intint1=1.0/3.0*A*x_knots[i]*x_knots[i]*x_knots[i]+1.0/2.0*B*x_knots[i]*x_knots[i]+C*x_knots[i];
                intint2=1.0/3.0*A*x_knots[i+1]*x_knots[i+1]*x_knots[i+1]+1.0/2.0*B*x_knots[i+1]*x_knots[i+1]+C*x_knots[i+1];
                sum2=sum2+intint2-intint1;

        }
        return sum2;
}

void solve(double** m,double* v,int len,double* output){
        using namespace std;
        int s;
        double *mb;
        int k=0;
        mb=(double*)malloc(sizeof(double)*len*len);
        for(int i=0;i<len;i++){
                for(int j=0;j<len;j++){
//                      printf("%lf\n",m[i][j]);
                        mb[k]=m[i][j];
                        k++;

                }

        }


        gsl_matrix_view m2=gsl_matrix_view_array(mb,len,len);
        gsl_vector_view v2=gsl_vector_view_array(v,len);
        gsl_vector *x=gsl_vector_alloc(len);
        gsl_permutation *p =gsl_permutation_alloc(len);
        gsl_linalg_LU_decomp(&m2.matrix,p,&s);
        gsl_linalg_LU_solve(&m2.matrix,p,&v2.vector,x);
        //printf("x=\n");

        //gsl_vector_fprintf(stdout,x,"%g");
        gsl_permutation_free(p);
        //gsl_vector_free(x);
        for(int i=0;i<len;i++){
                output[i]=gsl_vector_get(x,i);

        };
//      return *v;

}


double likelihoods(double* ytmp,double* xb,double* yb,int number_of_data,double* x_knots,int time_length,double* totaltotaltotal1){
        //double* ytmp;
        //double* yb;
        //double* xb;
        //double* x_knots;
        double* y_knots;
        double* h;
        double* v;
        double* u;

        double* mm1;
        double* sd1;
        //double* totaltotaltotal1;
        double* a;
        double* b;
        double* c;
        double* d;
        double** m;
        double lm=0.0;
        double lambda=0.0;
        double lm1=0.0;
        double lm2=0.0;
//      int time_length=10;
        int len;
//      int number_of_data=10;
        double delta=0.016;
        len=time_length-1;

//      ytmp=(double*) malloc(sizeof(double)*time_length);
//      yb=(double*) malloc(sizeof(double)*time_length);
//      xb=(double*) malloc(sizeof(double)*time_length);


        mm1=(double*) malloc(sizeof(double)*number_of_data);
        sd1=(double*) malloc(sizeof(double)*number_of_data);

//      totaltotaltotal1=(double*) malloc(sizeof(double)*time_length);

        a=(double*) malloc(sizeof(double)*time_length);
        b=(double*) malloc(sizeof(double)*time_length);
        c=(double*) malloc(sizeof(double)*time_length);
        d=(double*) malloc(sizeof(double)*time_length);
//      x_knots=(double*) malloc(sizeof(double)*time_length);
        y_knots=(double*) malloc(sizeof(double)*time_length);

        h=(double*) malloc(sizeof(double)*(time_length-1));

        v=(double*) malloc(sizeof(double)*(time_length-1));

        m=(double**) malloc(sizeof(double*)*(time_length-1));

        u=(double*) malloc(sizeof(double)*(time_length-2));
        for(int i=0;i<time_length-2;i++){
                m[i]=(double*)malloc(sizeof(double)*(time_length-1));

        }


//Inititalization

        for(int i=0;i<time_length;i++){

                //x_knots[i]=i;
        //      xb[i]=(i+0.4);
        //      totaltotaltotal1[i]=1.0;
        }

        for(int i=0;i<time_length;i++){

                y_knots[i]=i;

                //ytmp[i]=i;
        }
/*
        ytmp[0]=5;
        ytmp[1]=2;
        ytmp[2]=3;
        ytmp[3]=1;
        ytmp[4]=4;
        ytmp[5]=2;
        ytmp[6]=3;
        ytmp[7]=1;
        ytmp[8]=8;
        ytmp[9]=9;
*/
//Solve a natural spline

        for(int i=0;i<(time_length-1);i++){
                h[i]=x_knots[i+1]-x_knots[i];
        }

        for(int i=0;i<(time_length-2);i++){
                v[i]=6*((ytmp[i+2]-ytmp[i+1])/h[i+1]-(ytmp[i+1]-ytmp[i])/h[i]);
                //printf("%lf\n",v[i]);

        }

        for(int i=0;i<(time_length-2);i++){
                for(int j=0;j<(time_length-2);j++){
                        m[i][j]=0;

                }
        }


        m[0][0]=2*(h[0]+h[1]);
        m[0][1]=h[1];

        for(int i=1;i<(len-2);i++){
                m[i][i-1]=h[i];
                m[i][i]=2*(h[i]+h[i+1]);
                m[i][i+1]=h[i+2];

        }

        m[len-2][len-3]=h[len-3];
        m[len-2][len-2]=2*(h[len-3]+h[len-2]);


        for(int i=0;i<(time_length-2);i++){
                for(int j=0;j<(time_length-2);j++){
                //      m[i][j]=0;
                        //printf("%lf,",m[i][j]);

                }
                        //printf("\n");
        }

        solve(m,v,time_length-2,u);

        b[0]=0;
        a[0]=(u[0]-0)/6.0/(x_knots[2]-x_knots[1]);
        c[0]=(ytmp[1]-ytmp[0])/(x_knots[2]-x_knots[1])-1.0/6.0*(x_knots[2]-x_knots[1])*(+u[0]);
        d[0]=ytmp[0];
        for(int i=1;i<time_length-1;i++){
                b[i]=u[i-1]/2.0;
                a[i]=(u[i]-u[i-1])/6.0/(x_knots[i+1]-x_knots[i]);
                d[i]=ytmp[i];
                c[i]=(ytmp[i+1]-ytmp[i])/(x_knots[i+1]-x_knots[i])-1.0/6.0*(x_knots[i+1]-x_knots[i])*(2*u[i-1]+u[i]);

        }

        d[time_length-1]=ytmp[time_length-1];
//      double x[]={0.5,5.3,8.5,9.5};
//      double q[10];
//      double q2[10];
        f(xb,number_of_data,x_knots,time_length,a,b,c,d,mm1);

//      ff(x,4,x_knots,time_length,a,b,c,d,q2);
//      printf("hello\n");
        lm1=0.0;
        for(int i=0;i<number_of_data;i++){
                mm1[i]=mm1[i]*totaltotaltotal1[i];
                if(mm1[i]<0){
                        mm1[i]=0;

                }
                sd1[i]=sqrt(mm1[i]+delta*delta*(mm1[i]/totaltotaltotal1[i]/totaltotaltotal1[i]));
                //printf("q:%lf\n",mm1[i]);
//              printf("q2:%lf\n",q2[i]);
                if(yb[i]<16){
                        lm1=lm1+log(gsl_ran_poisson_pdf(yb[i],mm1[i]));
                }else{
                        lm1=lm1+log(gsl_ran_gaussian_pdf(yb[i]-mm1[i],sd1[i]));;

                }

        }

        lm2=ff_int(x_knots,time_length,a,b,c,d);

        printf("lm1:%lf lm2:%lf\n",lm1,lm2);
        lm=lm1+lambda*lm2;
        return lm;
}

int main(){
        int time_length=10;
        int number_of_data=10;
        double ytmp[10];
        double x_knots[10];
        double xb[10];

        double xb2[10];
        double totaltotaltotal1[10];

        for(int i=0;i<time_length;i++){

                x_knots[i]=i;
                xb[i]=(i+0.4);

                xb2[i]=(i);
                totaltotaltotal1[i]=1.0;
        }


        ytmp[0]=5;
        ytmp[1]=2;
        ytmp[2]=3;
        ytmp[3]=1;
        ytmp[4]=4;
        ytmp[5]=2;
        ytmp[6]=3;
        ytmp[7]=1;
        ytmp[8]=8;
        ytmp[9]=9;

        double yb[10]={5.5,2.2,2.8,1.0,4.2,2.2,3.1,1.2,7.5,8.8};

        double lm=likelihoods(ytmp,xb,yb,number_of_data,x_knots,time_length,totaltotaltotal1);
        printf("%lf\n",lm);

        double yb2[10]={5,2,3,1,4,2,3,1,8,9};

        lm=likelihoods(ytmp,xb2,yb2,number_of_data,x_knots,time_length,totaltotaltotal1);

        printf("%lf\n",lm);

}