// constructor, sets up data structures
Kalman() : dt(1.0) {
A.eye(6,6);
A(0,2) = A(1,3) = A(2,4) = A(3,5) = dt;
H.zeros(2,6);
H(0,0) = H(1,1) = 1.0;
Q.eye(6,6);
R = 1000 * eye(2,2);
xest.zeros(6,1);
pest.zeros(6,6);
}
void for_Algo(mat &A, mat &R, double h, int N, double tolerance,int max){
int counter=0;
int col = 0;
int row = 1;
Matrix2(A,h,N);
R.eye();
cout<<"--------------JACOBI ROTATION ALGORITHM--------------"<<endl;
clock_t start, finish;
start = clock();
maximum(A,N,col,row);
while (fabs(A(row,col))>tolerance && counter<max){
jacobi(A,R,N,col,row);
counter++;
maximum(A,N,col,row);
//TEST_MAX(A,N);
}
finish = clock();
float time=((float)finish - (float)start) / CLOCKS_PER_SEC;
cout<<counter<<" ripetitions and "<< time <<" seconds."<<endl;
minimumEigenvalues(A,N);
// cout<<"post-algorithm"<<endl;
// cout<<A<<endl;
return;
}
void exit_test(mat &A, mat &R, double h, int N){
Matrix2(A,h,N);
R.eye();
vec eigval(N);
Matrix2(A,h,N);
eig_sym(eigval, R, A);
A=diagmat(eigval);
//cout<<A<<endl;
//position of first eigenvalue
//out of ro
double ro=h;
for (int i=0;i<N;i++){
cout<<ro<<endl;
ro+=h;
}
cout<<"change"<<endl;
//out of first eigenvector
ro=h;
for(int i=0; i<N;i++){
cout<<R(i,0)*R(i,0)<<endl;
ro+=h;
}
cout<<"change"<<endl;
//out second eigenvector;
ro=h;
for(int i=0; i<N;i++){
cout<<R(i,1)*R(i,1)<<endl;
ro+=h;
}
cout<<"change"<<endl;
//out second eigenvector;
ro=h;
for(int i=0; i<N;i++){
cout<<R(i,2)*R(i,2)<<endl;
ro+=h;
}
cout<<"change"<<endl;
cout<<"break"<<endl;
}
void for_rotation(mat &A, mat &R, double h, int N, double tolerance,int max){
int counter=0;
int col = N-1;
int row = N-2;
Matrix(A,h,N);
R.eye();
cout<<"--------------JACOBI ROTATION MATRIX--------------"<<endl;
clock_t start, finish;
start = clock();
maximum(A,N,col,row);
while (fabs(A(row,col))>tolerance && counter<max){
jacobi_matrix(A,R,N,col,row);
counter++;
maximum(A,N,col,row);
}
finish = clock();
float time=((float)finish - (float)start) / CLOCKS_PER_SEC;
cout<<counter<<" ripetitions and "<< time <<" seconds."<<endl;
minimumEigenvalues(A,N);
return;
}
void exit(mat &A, mat &R, double h, int N, double tolerance,int max){
int counter=0;
int col = 0;
int row = 1;
Matrix2(A,h,N);
R.eye();
maximum(A,N,col,row);
while (fabs(A(row,col))>tolerance && counter<max){
jacobi(A,R,N,col,row);
counter++;
maximum(A,N,col,row);
}
// vec eigval(N);
// Matrix2(A,h,N);
// eig_sym(eigval, R, A);
//position of first eigenvalue
int m=0;
for (int i=0;i<N;i++){
if (fabs(A(i,i))<fabs(A(m,m))){m=i;}
}
//out of ro
double ro=h;
for (int i=0;i<N;i++){
cout<<ro<<endl;
ro+=h;
}
cout<<"change"<<endl;
//out of first eigenvector
ro=h;
for(int i=0; i<N;i++){
cout<<R(i,m)*R(i,m)<<endl;
ro+=h;
}
cout<<"change"<<endl;
//position second eigenvalue
int k=0;
for (int i=0;i<N;i++){
if (fabs(A(i,i))<fabs(A(k,k))&& A(i,i)!=A(m,m)){k=i;}
}
//out second eigenvector;
ro=h;
for(int i=0; i<N;i++){
cout<<R(i,k)*R(i,k)<<endl;
ro+=h;
}
cout<<"change"<<endl;
//position third eigenvalue
int l=0;
for (int i=0;i<N;i++){
if (fabs(A(i,i))<fabs(A(l,l))&& A(i,i)!=A(m,m)&&A(i,i)!=A(k,k)){l=i;}
}
//out second eigenvector;
ro=h;
for(int i=0; i<N;i++){
cout<<R(i,l)*R(i,l)<<endl;
ro+=h;
}
cout<<"change"<<endl;
cout<<"break"<<endl;
}