int main(int argc, char *argv[]) {
if(argc !=3 ) {
printf("Usage:\n<program_name> <rows> <columns>\n");
return -1;
}
int r=atoi(argv[1]);
int c=atoi(argv[2]);
int **matrix1 = createMatrix(r,c);
int **matrix2 = createMatrix(r,c);
printf("Matrix 1 :\n");
initMatrix(matrix1, r,c);
displayMatrix(matrix1, r,c);
printf("Matrix 2 :\n");
initMatrix(matrix2, r,c);
displayMatrix(matrix2, r,c);
int **sum_matrix3 = sumMatrix(matrix1, matrix2, r, c);
deleteMatrix(matrix1,r,c);
deleteMatrix(matrix2,r,c);
printf("\nSum Matrix 3 :\n");
displayMatrix(sum_matrix3, r, c);
deleteMatrix(sum_matrix3,r,c);
return 0;
}
void DFG::calcFactorMarginals(vector<xmatrix_t> & factorMarginals, vector<vector<xvector_t const *> > & inMessages) const
{
if (factorMarginals.size() == 0)
initFactorMarginals(factorMarginals);
for (unsigned facId = 0; facId < factors.size(); facId++) {
unsigned ndId = convFacToNode(facId);
vector<xvector_t const *> const & inMes = inMessages[ndId];
xmatrix_t & m = factorMarginals[facId];
m = nodes[ndId].potential; // set facMar = potential
if (nodes[ndId].dimension == 1)
for (unsigned i = 0; i < m.size2(); i++)
m(0, i) *= (*inMes[0])[i];
else if (nodes[ndId].dimension == 2)
for (unsigned i = 0; i < m.size1(); i++)
for (unsigned j = 0; j < m.size2(); j++)
m(i, j) *= (*inMes[0])[i] * (*inMes[1])[j];
else // should not happen
errorAbort("Factor with more than two neighbors. Aborts.");
// normalize
}
xnumber_t const Z = sumMatrix(factorMarginals[0]);
for (unsigned facId = 0; facId < factors.size(); facId++)
factorMarginals[facId] *= 1/ Z;
}
void
gaussianEstimator_Pred ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, Matrix (*afun)(Matrix m, float t), float *dt, Matrix *m_opt)
{
float D = elem(sizeOfMatrix(*m_opt),0,1)+1; //printf("%f\n", D);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = elem(sizeOfMatrix(*xEst),0,0); //printf("%f\n", Nx);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
/* Eigenvectors, Eigenvalues */
int dimC = elem ( sizeOfMatrix(*CEst), 0, 0 );
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val );
/* m1 = vec*sqrtf(val) */
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val);
freeMatrix(Vec);
freeMatrix(Val);
/* rotate & scale samples: m = m1*S */
Matrix m = scaledSamplePoints(m1, *m_opt);
/* x = x*ones(1,d) */
Matrix x = fillMatrix(*xEst, d);
/* shift samples: m = m + x */
m = addMatrix(m, x); //printMatrix(m);
/* afun */
/* Prediction: mean
xPredDiracs = feval(afun,m, [], [], t);
xPred = w*sum(xPredDiracs, 2);*/
Matrix xPredDiracs = (*afun) (m, *dt); //printMatrix(xPredDiracs);
Matrix xPredDiracsSum = sumMatrix(xPredDiracs, 2); //printMatrix(xPredDiracsSum);
Matrix xPred = mulScalarMatrix(w, xPredDiracsSum); //printMatrix(xPred);
//mxDiracs = xPredDiracs-repmat(xPred, 1, d);
//CPred = w_opt*mxDiracs*mxDiracs';
Matrix mxDiracs = subMatrix(xPredDiracs, fillMatrix(xPred, d)); //printMatrix(mxDiracs);
Matrix CPred = mulScalarMatrix(w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs))); //printMatrix(CPred);
//RETURN
*xEst = xPred; //printMatrix(*xEst);
*CEst = CPred; //printMatrix(*CEst);
freeMatrix(m);
freeMatrix(xPredDiracs);
freeMatrix(xPredDiracsSum);
}
void summa(MPI_Comm comm_cart, const int m_block, const int n_block, const int k_block, double A_local[], double B_local[], double C_local[]) {
// determine my cart coords
int coords[2];
MPI_Cart_coords(comm_cart, rank, 2, coords);
const int my_row = coords[0];
const int my_col = coords[1];
int belongs[2];
// create row comms for A
MPI_Comm row_comm;
belongs[0] = 0;
belongs[1] = 1;
MPI_Cart_sub(comm_cart, belongs, &row_comm);
// create col comms for B
MPI_Comm col_comm;
belongs[0] = 1;
belongs[1] = 0;
MPI_Cart_sub(comm_cart, belongs, &col_comm);
/*int row_rank, col_rank;
MPI_Comm_rank(row_comm, &row_rank);
MPI_Comm_rank(col_comm, &col_rank);
if(rank == 1) std::cout << "Rank: " << rank << "-->(" << my_col << "," << my_row << ") (" << col_rank << "," << row_rank << ")" << std::endl;
//printf("Rank: %i-->(%i,%i)|(%i,%i)\n", rank, my_col, my_row, col_rank, row_rank);*/
double * A_saved = (double *) calloc(m_block * n_block, sizeof(double));
double * B_saved = (double *) calloc(n_block * k_block, sizeof(double));
double * C_tmp = (double *) calloc(m_block * k_block, sizeof(double));
memcpy(A_saved, A_local, m_block * n_block * sizeof(double));
memcpy(B_saved, B_local, n_block * k_block * sizeof(double));
int number_blocks = n / n_block;
for(int broadcaster = 0; broadcaster < number_blocks; ++broadcaster){
if (my_col == broadcaster) {
memcpy(A_local, A_saved, m_block * n_block * sizeof(double));
}
MPI_Bcast(A_local, m_block * n_block, MPI_DOUBLE, broadcaster, row_comm);
if (my_row == broadcaster) {
memcpy(B_local, B_saved, n_block * k_block * sizeof(double));
}
MPI_Bcast(B_local, n_block * k_block, MPI_DOUBLE, broadcaster, col_comm);
multMatricesLineByLine(m_block, n_block, k_block, A_local, B_local, C_tmp);
sumMatrix(m_block, n_block, C_local, C_tmp, C_local);
}
}
int GlobalNormFactor::optimizeParametersImpl()
{
m_ = counts_;
if (pseudoCounts_.size1() != 0)
m_ += pseudoCounts_;
number_t n = sumMatrix(m_);
if (n == 0.0)
errorAbort("GlobalNormFactor:optimizeParameters: no counts: optimization not possible.");
m_ *= 1.0 / n;
return 1;
}
void
gaussianEstimator_Est (Matrix *xEst, Matrix *CEst, Matrix *y, Matrix *Cv, Matrix (*hfun)(Matrix m), Matrix *m_opt)
{
//printMatrix(*xEst);
//printMatrix(*CEst);system("PAUSE");
Matrix tmp = sizeOfMatrix(*m_opt);
float D = elem(tmp,0,1)+1; //printf("%f\n", D);
freeMatrix(tmp);
float w_opt = 1/D; //printf("%f\n", w_opt);
tmp = sizeOfMatrix(*xEst);
float Nx = elem(tmp,0,0); // printf("%f\n", Nx);
freeMatrix(tmp);
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; // printf("%f\n", w);system("PAUSE");
// Eigenvectors, Eigenvalues
tmp = sizeOfMatrix(*CEst);
int dimC = elem ( tmp, 0, 0 );
freeMatrix(tmp);
Matrix Vec = zeroMatrix(dimC, dimC);
Matrix Val = zeroMatrix(dimC, dimC);
eig ( CEst, &Vec, &Val ); //printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
Matrix m1 = mulMatrix(Vec, Val); //printMatrix(m1); system("PAUSE");
freeMatrix(Vec);
freeMatrix(Val);
//* rotate & scale samples: m = m1*S
Matrix m = scaledSamplePoints(m1, *m_opt); // printMatrix(m); system("PAUSE");
Matrix mxDiracs = mulScalarMatrix(1, m);
//* x = x*ones(1,d)
Matrix x = fillMatrix(*xEst, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ) ; //printMatrix(m);
freeMatrix(tmp);
//% Predicted Measurements
//* hfun
// yPredDiracs = feval(hfun, m, [], [], t);
// yPred = w*sum(yPredDiracs, 2);
Matrix yPredDiracs = (*hfun) (m); //printMatrix(yPredDiracs );
Matrix yPredDiracsSum = sumMatrix(yPredDiracs, 2);
Matrix yPred = mulScalarMatrix(w, yPredDiracsSum);
// myDiracs = yPredDiracs-repmat(yPred, 1, d);
tmp = fillMatrix(yPred, d);
Matrix myDiracs = subMatrix(yPredDiracs, tmp);
freeMatrix(tmp);
//* CPred = w_opt*mxDiracs*mxDiracs';
// Matrix CPred = mulScalarMatrix( w_opt, mulMatrix(mxDiracs, transposeMatrix(mxDiracs)) );
// Matrix CPred = *CEst;
// Cxy = w_opt*mxDiracs*myDiracs';
Matrix tmp1 = transposeMatrix(myDiracs);
Matrix tmp2 = mulMatrix(mxDiracs, tmp1);
Matrix Cxy = mulScalarMatrix( w_opt, tmp2);
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = w_opt*myDiracs*myDiracs'+Cv;
tmp1 = transposeMatrix(myDiracs);
tmp2 = mulMatrix(myDiracs, tmp1);
Matrix tmp3 = mulScalarMatrix( w_opt, tmp2);
Matrix Cy = addMatrix( tmp3 , *Cv );
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
// K = Cxy / Cy;
tmp = invertCovMatrix(Cy);
Matrix K = mulMatrix( Cxy, tmp);
freeMatrix(tmp);
// I = y - yPred;
Matrix I = subMatrix( *y, yPred );
// xEst = xPred + K*I;
tmp = mulMatrix( K, I );
Matrix tmp23 = addMatrix( *xEst, tmp);
appMatrix(*xEst,0,5,0,0, tmp23,0,5,0,0);
freeMatrix(tmp);
// CEst = CPred - K*Cy*K';
tmp1 = mulMatrix(K, Cy);
tmp2 = transposeMatrix(K);
tmp3 = mulMatrix( tmp1, tmp2);
Matrix tmp24 = subMatrix(*CEst, tmp3);
appMatrix(*CEst,0,5,0,5, tmp24,0,5,0,5);
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp24);
freeMatrix(m1);
freeMatrix(m);
freeMatrix(mxDiracs);
freeMatrix(x);
freeMatrix(yPredDiracs);
freeMatrix(yPredDiracsSum);//
freeMatrix(yPred);//
freeMatrix(myDiracs);
freeMatrix(Cxy);
freeMatrix(Cy);
freeMatrix(K);//
freeMatrix(I);//
freeMatrix(tmp23);
}
/**
* Multiply two matrix and store the result into the result matrix given by parameter
*/
void multiplyCoolMatrix(Matrix matrixA, Matrix matrixB, Matrix* matrixResult) {
int matrixSize = getSize(matrixA);
createSquareMatrix(matrixResult, matrixSize);
if (matrixSize > 2) {
int subMatrixSize = matrixSize / 2;
// Auxiliar variables
Matrix auxMatrixA1 = NULL, auxMatrixB1 = NULL, auxMatrixA2 = NULL, auxMatrixB2 = NULL;
Matrix auxContainer1 = NULL, auxContainer2 = NULL, solutionContainer = NULL;
createSquareMatrix(&solutionContainer, subMatrixSize);
createSquareMatrix(&auxMatrixA1, subMatrixSize);
createSquareMatrix(&auxMatrixB1, subMatrixSize);
createSquareMatrix(&auxMatrixA2, subMatrixSize);
createSquareMatrix(&auxMatrixB2, subMatrixSize);
createSquareMatrix(&auxContainer1, subMatrixSize);
createSquareMatrix(&auxContainer2, subMatrixSize);
int operation1Index = 0;
for(short quadrant = 0; quadrant < 4; quadrant++) {
operation1Index += 4;
copyQuadrant(matrixA, &auxMatrixA1, operations[operation1Index -4]);
copyQuadrant(matrixB, &auxMatrixB1, operations[operation1Index -3]);
multiplyCoolMatrix(auxMatrixA1, auxMatrixB1, &auxContainer1);
copyQuadrant(matrixA, &auxMatrixA2, operations[operation1Index -2]);
copyQuadrant(matrixB, &auxMatrixB2, operations[operation1Index -1]);
multiplyCoolMatrix(auxMatrixA2, auxMatrixB2, &auxContainer2);
sumMatrix(auxContainer1, auxContainer2, &solutionContainer);
assignCuadrant(solutionContainer, matrixResult, quadrant);
}
destroyMatrix(&auxMatrixA1);
destroyMatrix(&auxMatrixB1);
destroyMatrix(&auxMatrixA2);
destroyMatrix(&auxMatrixB2);
destroyMatrix(&auxContainer1);
destroyMatrix(&auxContainer2);
destroyMatrix(&solutionContainer);
} else {
int value1, value2, container = 0;
int matrixSize = getSize(matrixA);
for (int i = 0; i < matrixSize; i++) {
for (int j = 0; j < matrixSize; j++) {
for (int k= 0; k < matrixSize; k++) {
getValue(matrixA, j, k, &value1);
getValue(matrixB, k, i, &value2);
container += value1 * value2;
}
assingValue(matrixResult, j, i, container);
container = 0;
}
}
}
}
int main(int argc, char** argv)
{
// Argument parsing.
Matrix* mat[2];
int matNum = 0;
int dumpInputMatrices = 0;
char* outFileName = 0;
for(int a = 1; a < argc; ) {
if( (strcmp(argv[a], "-random") == 0)
&& (a + 2 < argc)
&& (matNum < 2))
{
a++;
int width = 0;
int height = 0;
if(sscanf(argv[a++], "%d", &width) != 1) {
printf("mmult: can't parse matrix width\n");
exit(1);
}
if(sscanf(argv[a++], "%d", &height) != 1) {
printf("mmult: can't parse matrix height\n");
exit(1);
}
mat[matNum++] = newRandomMatrix (width, height);
}
else if ( (strcmp(argv[a], "-out") == 0)
&& (a + 1 < argc))
{
a++;
outFileName = argv[a++];
}
else if ( strcmp(argv[a], "-dumpinput") == 0)
{
a++;
dumpInputMatrices = 1;
}
else badUsage();
}
if (matNum != 2)
badUsage();
// Alloc the destination matrix.
Matrix* matDest = newZeroMatrix (mat[1]->width, mat[0]->height);
// Do the dead.
struct benchtime *bt = bench_begin();
mmult(matDest, mat[0], mat[1]);
bench_done(bt);
// Write out matrices as files, if we were asked for them
if (dumpInputMatrices) {
char name[80];
snprintf(name, 80, "input1-%dx%d.mat", mat[0]->width, mat[0]->height);
writeMatrixAsTextFile(name, mat[0]);
snprintf(name, 80, "input2-%dx%d.mat", mat[1]->width, mat[1]->height);
writeMatrixAsTextFile(name, mat[1]);
}
if(outFileName != 0)
writeMatrixAsTextFile(outFileName, matDest);
// Dump checksum
printf("sum = %f\n", sumMatrix(matDest));
}
// [[Rcpp::export]]
RcppExport SEXP getMLE(SEXP mtx, SEXP steps, SEXP epsilon){
NumericMatrix mt(mtx);
const NumericVector st(steps);
const NumericVector eps(epsilon);
const size_t stps=st[0];
const double epsil=eps[0];
const NumericVector * gprimePrior=E(mt);
NumericVector * gprime= new NumericVector(*gprimePrior);
delete gprimePrior;
NumericMatrix track(stps,2);
fillInMatrix(track,0);
double gDist(sumDist(mt,*gprime));
double deltaG=0;
R_len_t counter=0;
while ((counter < stps) && (deltaG > epsil)){
track(counter,0)=gDist;
track(counter,1)=deltaG;
NumericMatrix * devisable = new NumericMatrix(mt.nrow(),mt.ncol());
for(int i=0;i<mt.nrow();i++){
NumericVector row=mt(i,_);
const NumericVector * ddr = diffDistRatio(row,*gprime);
(*devisable)(i,_)=*ddr;
delete ddr;
}
const NumericVector * rs=rowSums(*devisable);
delete devisable;
NumericMatrix * devider = new NumericMatrix(mt.nrow(),mt.ncol());
for(int i=0;i<mt.nrow();i++){
NumericVector row=mt(i,_);
const NumericVector * ddr = diffDistInverse(row,*gprime);
(*devisable)(i,_)=*ddr;
delete ddr;
}
const double suDiv=sumMatrix(*devider);
delete devider;
for(int i=0;i<gprime->size();i++){
(*gprime)[i]=(*rs)[i]/suDiv;
}
delete rs;
gDist=double(sumDist(mt,*gprime));
deltaG=fabs(track(counter,0)-gDist);
counter++;
}
List ret;
ret.push_back(*gprime);
delete gprime;
ret.push_back(track);
return ret;
}
Matrix callFunc(const char *funcName, ASTNode *argListNode){
/* sort out args */
ASTNode *argNode[10];
Matrix result;
Number calcresult;
MatList *p;
if (strcmp(funcName, "eye")==0) { /* generate eye matrix */
argNode[0] = getNthArgFromArgList(argListNode,1);
result = createEyeMatrix((int)readOneElementOfMatrix(argNode[0]->mat,1,1));
}
else if(strcmp(funcName, "rand")==0){/*generate rand matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
argNode[1] = getNthArgFromArgList(argListNode,2);
result = createRandMatrix((int)readOneElementOfMatrix(argNode[0]->mat,1,1),readOneElementOfMatrix(argNode[1]->mat,1,1));
}
else if(strcmp(funcName, "zeros")==0){/*generate rand matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
argNode[1] = getNthArgFromArgList(argListNode,2);
result = createZeroMatrix((int)readOneElementOfMatrix(argNode[0]->mat,1,1),readOneElementOfMatrix(argNode[1]->mat,1,1));
}
else if(strcmp(funcName, "max")==0){/*calculate maximum of matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
calcresult = maxMatrix(argNode[0]->mat);
result = createEyeMatrix(1);
changeOneElementOfMatrix(result, 1, 1, calcresult);
}
else if(strcmp(funcName, "min")==0){/*calculate minimum of matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
calcresult = minMatrix(argNode[0]->mat);
result = createEyeMatrix(1);
changeOneElementOfMatrix(result, 1, 1, calcresult);
}
else if(strcmp(funcName, "sum")==0){/*calculate sum of matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
calcresult = sumMatrix(argNode[0]->mat);
result = createEyeMatrix(1);
changeOneElementOfMatrix(result, 1, 1, calcresult);
}
else if(strcmp(funcName, "round")==0){/*calculate round of matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
result = roundMatrix(argNode[0]->mat);
}
else if(strcmp(funcName, "upper")==0){/*calculate upper of matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
result = upperMatrix(argNode[0]->mat);
}
else if(strcmp(funcName, "lower")==0){/*calculate upper of matrix*/
argNode[0] = getNthArgFromArgList(argListNode,1);
result = lowerMatrix(argNode[0]->mat);
}
else if(strcmp(funcName, "det")==0){/*calculate DeterminantCalc*/
argNode[0] = getNthArgFromArgList(argListNode,1);
calcresult = DeterminantCalc(argNode[0]->mat.row, argNode[0]->mat);
result = createEyeMatrix(1);
changeOneElementOfMatrix(result, 1, 1, calcresult);
}
else if(strcmp(funcName, "turn")==0){/*calculate turn*/
argNode[0] = getNthArgFromArgList(argListNode,1);
result = turnMatrix(argNode[0]->mat);
}
else if(strcmp(funcName, "power")==0){/*calculate power*/
argNode[0] = getNthArgFromArgList(argListNode,1);
argNode[1] = getNthArgFromArgList(argListNode,2);
result = powerMatrix(argNode[0]->mat,(int)readOneElementOfMatrix(argNode[1]->mat,1,1));
}
else if(strcmp(funcName, "dot")==0){/*calculate dot*/
argNode[0] = getNthArgFromArgList(argListNode,1);
argNode[1] = getNthArgFromArgList(argListNode,2);
result = dotMatrix(argNode[0]->mat,argNode[1]->mat);
}
else if(strcmp(funcName, "norm")==0){/*calculate norm*/
argNode[0] = getNthArgFromArgList(argListNode,1);
result = normMatrix(argNode[0]->mat);
}
else if(strcmp(funcName, "angle")==0){/*calculate norm*/
argNode[0] = getNthArgFromArgList(argListNode,1);
argNode[1] = getNthArgFromArgList(argListNode,2);
result = angleMatrix(argNode[0]->mat,argNode[1]->mat);
}
else{
if (get_submatrix == 1){
p = checkMatName(funcName);
argNode[0] = getNthArgFromArgList(argListNode,1);
argNode[1] = getNthArgFromArgList(argListNode,2);
result = createSubMatrix(p->mat,argNode[0]->mat,argNode[1]->mat);
get_submatrix = 0;
}
else{
p = checkMatName(funcName);
argNode[0] = getNthArgFromArgList(argListNode,1);
argNode[1] = getNthArgFromArgList(argListNode,2);
calcresult = readOneElementOfMatrix(p->mat,(int)readOneElementOfMatrix(argNode[0]->mat,1,1),readOneElementOfMatrix(argNode[1]->mat,1,1));
result = createEyeMatrix(1);
changeOneElementOfMatrix(result, 1, 1, calcresult);
}
}
return result;
}
int kalman_function(sensor_struct_t* sen,float_j_t dt){
/*
*
* system model
* omega = [ w_x w_y w_z ]^T
* x = [pi theta psi]^T
* used x_dot, C_inv, omega, x_k_1, x_k, dt,
*
* */
#define Def_x 0
#define Def_y 1
#define Def_z 2
#define Def_pi 0
#define Def_theta 1
#define Def_psi 2
//float_j_t dt;
//float_j_t B[3][3]={0,};
float_j_t res[3][3]={0,};
float_j_t res2[3][3]={0,};
float_j_t omega[3][3]={0,};
omega[0][0] = sen->gyroX*DEG_TO_RAD;
omega[1][0] = sen->gyroY*DEG_TO_RAD;
omega[2][0] = sen->gyroZ*DEG_TO_RAD;
static float_j_t x_k_1[3][3]={0,};
float_j_t x_k_p[3][3]={0,};
float_j_t pi,theta,psi;
pi = x_k_1[Def_pi][0];
theta = x_k_1[Def_theta][0];
psi = x_k_1[Def_psi][0];
if(cosf(theta)==0.0){
printf("cosf error\n ");
theta += 0.00001;
}
float_j_t C_inv[3][3]={0,};
C_inv[0][0] = 1*dt;
C_inv[0][1] = sinf(pi)*tanf(theta)*dt;
C_inv[0][2] = cosf(pi)*tanf(theta)*dt;
C_inv[1][0] = 0;
C_inv[1][1] = cosf(pi)*dt;
C_inv[1][2] = -sinf(pi)*dt;
C_inv[2][0] = 0;
C_inv[2][1] = (sinf(pi)/cosf(theta))*dt;
C_inv[2][2] = (cosf(pi)/cosf(theta))*dt;
//x_k = x_k_1 + C_inv * omega * dt
multiMatrix(C_inv,omega,res,3,3);
sumMatrix(res,x_k_1,x_k_p,3,3);
/* A_k*/
float_j_t A[3][3]={0,};
A[0][0] = 1 + (sinf(theta)*(omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt;
A[0][1] = ((omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt;
A[0][2] = 0;
A[1][0] = (-omega[1][0]*sinf(pi)-omega[2][0]*cosf(pi))*dt;
A[1][1] = 1;
A[1][2] = 0;
A[2][0] = ((omega[1][0]*cosf(pi)-omega[2][0]*sinf(pi)) / cosf(theta))*dt;
A[2][1] = (sinf(theta)*(omega[1][0]*sinf(pi)+omega[2][0]*cosf(pi)) / (cosf(theta)*cosf(theta)))*dt;
A[2][2] = 1;
float_j_t P_k_p[3][3]={0,};
static float_j_t P_k_1[3][3]={1,0,0,0,1,0,0,0,1};
float_j_t Q[3][3]={1,0,0,0,1,0,0,0,1};
//P_k = A* P_k_1 * A^T + B * Q * B^T
similarMatrix(A,P_k_1,res,3,3);
similarMatrix(C_inv,Q,res2,3,3);
sumMatrix(res,res2,P_k_p,3,3);
//Correct term
float_j_t H[3][3]={1,0,0,0,1,0,0,0,0};
float_j_t norm_a=9.8;
norm_a=sqrtf(powf(sen->accX,2)+powf(sen->accY,2)+powf(sen->accZ,2));
// printf("norm_a = %f\n",norm_a);
float_j_t R[3][3]={0,};
if((norm_a<10.8 )&&(norm_a>8.8)){
R[0][0]=1;
R[1][1]=1;
}else{
R[0][0]=1000;
R[1][1]=1000;
}
float_j_t res3[3][3] = {0,};
similarMatrix(H,P_k_p,res,3,3);
//printf("similarMatrix res\n");
//printMatrix(res,3,3);
sumMatrix(res,R,res2,3,3);
_2by2_inverseMatrix(res2,res,3,3);
//inverseMatrix(res2,res,3,3);
//result inverse matrix is res
//so kalman gain K = Pk* H^T* res
float_j_t K[3][3]={0,};
transMatrix(H,res3,3,3);
multiMatrix(P_k_p,res3,res2,3,3);
res2[0][2]=0;
res2[1][2]=0;
res2[2][0]=0;
res2[2][1]=0;
res2[2][2]=0;
// printf("res2 gain\n");
// printMatrix(res2,3,3);
// printf("res gain\n");
// printMatrix(res,3,3);
multiMatrix(res2,res,K,3,3);
// printf("kalman gain\n");
// printMatrix(K,3,3);
/* state variable, covaliance mat update*/
float_j_t x_k[3][3]={0,};
//calculate z_k roll, ptich, with gravitational euler angle calc.
float_j_t z_k[3][3] ={0,};
#if 1
z_k[0][0] = atan2f(sen->accY,sen->accZ);
z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) ));
#else
if(sen->accZ>=0){
z_k[0][0] = atan2f(sen->accY,sen->accZ);
z_k[1][0] = atan2f(-sen->accX, sqrtf( powf(sen->accY,2)+powf(sen->accZ,2) ));
}else if(sen->accZ<0){
omega[1][0] = -omega[1][0];
z_k[1][0] = -atan2f(sen->accX,sen->accZ);
z_k[0][0] = atan2f(-sen->accY, sqrtf( powf(sen->accX,2)+powf(sen->accZ,2) ));
}
#endif
//GravityToEuler(sen->accX, sen->accY, sen->accZ, z_k);
//printf("r %f p %f ",z_k[0][0]*180/3.14,z_k[1][0]*180/3.14);
multiMatrix(H,x_k_p,res,3,3);
subMatrix(z_k,res,res2,3,3);
multiMatrix(K,res2,res,3,3);
sumMatrix(x_k_p,res,x_k,3,3);
float_j_t P_k[3][3]={0,};
multiMatrix(K,H,res,3,3);
multiMatrix(res,P_k_p,res2,3,3);
subMatrix(P_k_p,res2,P_k,3,3);
/*insert prev value*/
insertMatrix(x_k, x_k_1,3,3);
insertMatrix(P_k, P_k_1,3,3);
/*Debug print*/
//printf("roll = %f , pitch = %f \n",x_k[0][0]*180.0/3.14,x_k[1][0]*180.0/3.14);
sen->roll=x_k[0][0]*RAD_TO_DEG;
sen->pitch=x_k[1][0]*RAD_TO_DEG;
return 0;
}
