void extended_kalman_init( uFloat **P, uFloat *x )
{
#ifdef PRINT_DEBUG
printf( "ekf: Initializing filter\n" );
#endif
alloc_globals( STATE_SIZE, MEAS_SIZE );
sys_transfer = matrix( 1, STATE_SIZE, 1, STATE_SIZE );
mea_transfer = matrix( 1, MEAS_SIZE, 1, STATE_SIZE );
/* Init the global variables using the arguments. */
vecCopy( x, state_post, STATE_SIZE );
vecCopy( x, state_pre, STATE_SIZE );
matCopy( P, cov_post, STATE_SIZE, STATE_SIZE );
matCopy( P, cov_pre, STATE_SIZE, STATE_SIZE );
covarianceSet( sys_noise_cov, mea_noise_cov );
#ifdef PRINT_DEBUG
printf( "ekf: Completed Initialization\n" );
#endif
}
/******************************************************************************
* For floating point operation, a 3*3 matrix inversion takes about 180 - 200us.
*|a11 a12 a13|-1 | a33a22-a32a23 -(a33a12-a32a13) a23a12-a22a13 |
*|a21 a22 a23| = 1/DET * | -(a33a21-a31a23) a33a11-a31a13 -(a23a11-a21a13) |
*|a31 a32 a33| | a32a21-a31a22 -(a32a11-a31a12) a22a11-a21a12 |
******************************************************************************/
float matInverse(Matrix dst, Matrix src) {
float det = matDet(src);
if (det == 0) return det;
float det_inv = 1/det;
float **a = src->data;
Matrix temp = matCreate(src->row_size, src->col_size, src->type);
if (src->row_size == 2 && src->col_size == 2) {
temp->data[0][0] = a[1][1]*det_inv;
temp->data[0][1] = a[1][0]*det_inv;
temp->data[1][0] = a[0][1]*det_inv;
temp->data[1][1] = a[0][0]*det_inv;
} else if (src->row_size == 3 && src->col_size == 3) {
temp->data[0][0] = (a[2][2] * a[1][1] - a[2][1] * a[1][2])*det_inv;
temp->data[0][1] = (a[2][1] * a[0][2] - a[2][2] * a[0][1])*det_inv;
temp->data[0][2] = (a[1][2] * a[0][1] - a[1][1] * a[0][2])*det_inv;
temp->data[1][0] = (a[2][0] * a[1][2] - a[2][2] * a[1][0])*det_inv;
temp->data[1][1] = (a[2][2] * a[0][0] - a[2][0] * a[0][2])*det_inv;
temp->data[1][2] = (a[1][0] * a[0][2] - a[1][2] * a[0][0])*det_inv;
temp->data[2][0] = (a[2][1] * a[1][0] - a[2][0] * a[1][1])*det_inv;
temp->data[2][1] = (a[2][0] * a[0][1] - a[2][1] * a[0][0])*det_inv;
temp->data[2][2] = (a[1][1] * a[0][0] - a[1][0] * a[0][1])*det_inv;
} else {
return 0;
}
matCopy(dst, temp);
free(temp);
return det;
}
void flipV(int mat[4][4])
{
int i, n;
int tempMat[4][4];
matCopy(mat, tempMat);
for (i = 0; i < 4; i++)
for (n = 0; n < 4; n++)
mat[3-n][i] = tempMat[n][i];
}
void transposMain(int mat[4][4])
{
int i, n;
int tempMat[4][4];
matCopy(mat, tempMat);
for (i = 0; i < 4; i++)
for (n = 0; n < 4; n++)
mat[n][i] = tempMat[i][n];
}
void rotateCW90(int mat[4][4])
{
int i, n;
int tempMat[4][4] = { 0 };
matCopy(mat, tempMat);
for (i = 0; i < 4; i++)
for (n = 0; n < 4; n++)
mat[n][3 - i] = tempMat[i][n];
}
static void take_inverse( uFloat **in, uFloat **out, int n )
{
#ifdef PRINT_DEBUG
printf( "ekf: calculating inverse\n" );
#endif
/* Nothing fancy for now, just a Gauss-Jordan technique,
with good pivoting (thanks to NR). */
gjInverse( in, n, out, 0 ); /* out is SCRATCH */
matCopy( in, out, n, n );
}
void matTranspose(Matrix dst, Matrix src) {
Matrix temp = matCreate(src->row_size, src->col_size, src->type);
unsigned char row, col;
for(row = 0; row < src->row_size; ++row) {
for(col = 0; col < src->col_size; ++col) {
temp->data[col][row] = src->data[row][col];
}
}
matCopy(dst, temp);
free(temp);
}
void matDotProduct(Matrix dst, Matrix src1, Matrix src2) {
unsigned char row, col, k;
float **a = src1->data;
float **b = src2->data;
Matrix temp = matCreate(src1->row_size, src2->col_size, src1->type);
for(row = 0; row < temp->row_size; ++row) {
for(col = 0; col < temp->col_size; ++col) {
temp->data[row][col] = 0;
for(k = 0; k < src1->col_size; ++k) {
temp->data[row][col] += a[row][k] * b[k][col];
}
}
}
matCopy(dst, temp);
free(temp);
}
int
Simplex::solve(std::vector<double>& rhs) const {
// save the input matrix and rhs vector so we get a chance to
// recover in case of a singular matrix.
const double residualTol = 1.e-8;
std::vector<double> mat(mnpdims * mndims);
// build the matrix system
for (size_t jcol = 0; jcol < mnpdims; ++jcol) {
for (size_t irow = 0; irow < mndims; ++irow) {
mat[irow + jcol*mndims] = mvertices[jcol + 1][irow] - mvertices[0][irow];
}
}
std::vector<double> matCopy(mnpdims * mndims);
std::vector<double> bCopy(rhs.size());
std::copy(mat.begin(), mat.end(), matCopy.begin());
std::copy(rhs.begin(), rhs.end(), bCopy.begin());
int nrow = (int) mndims;
int ncol = (int) mnpdims;
char t = 'n';
int one = 1;
int mn = ncol;
int nb = 1; // optimal block size
int lwork = mn + mn*nb;
std::vector<double> work((size_t) lwork);
int errCode = 0;
_GELS_(&t, &nrow, &ncol, &one,
&matCopy.front(), &nrow,
&rhs.front(), &nrow,
&work.front(), &lwork, &errCode);
if (!errCode) {
// merrCode == 0 indicates everything was fine
// merrCode < 0 indicates bad entry
// in either case return
return errCode;
}
else if (errCode > 0) {
if (nrow <= 1) {
return errCode;
}
for (size_t i = 0; i < mndims; ++i) {
rhs[i] = bCopy[i];
}
errCode = 0;
// relative accuracy in the matrix data
double rcond = std::numeric_limits<double>::epsilon();
int rank;
std::vector<int> jpvt(ncol);
lwork = mn + 3*ncol + 1 > 2*mn + nb*1? mn + 3*ncol + 1: 2*mn + nb*1;
work.resize(lwork);
_GELSY_(&nrow, &ncol, &one,
&matCopy.front(), &nrow,
&rhs.front(), &nrow,
&jpvt.front(), &rcond, &rank, &work.front(), &lwork, &errCode);
// check if this is a good solution
double residualError = 0.0;
for (size_t i = 0; i < mndims; ++i) {
double rowSum = 0.0;
for (size_t j = 0; j < mnpdims; ++j) {
rowSum += mat[i + nrow*j]*rhs[j];
}
residualError += std::abs(rowSum - bCopy[i]);
}
if (residualError < residualTol) {
// good enough
errCode = 1;
return errCode;
}
// some error
errCode = 2;
return errCode;
}
// we should never reach that point
errCode = 0;
return errCode;
}