void solveCircleSummation() { int nTests; scanf(" %d", &nTests); for(int test = 0; test < nTests; ++test) { int N; int mSize; scanf(" %d %d", &N, &mSize); vector<ullong> vec; for(int entry = 0; entry < N; ++entry) { int inp; scanf(" %d", &inp); vec.push_back(inp); } //printf("%d %d\n",N, mSize); SquareMatrix sqMat = SquareMatrix(N, mSize); int N_1 = mSize%N; vector<ullong> finalOut(N, 0); for(int entry = 0; entry < N; ++entry) { sqMat.multiply(vec, finalOut); print(finalOut, N_1); printf("\n"); N_1 = (N_1 + 1) % N; rotateBy1(vec); } if(test < nTests -1) printf("\n"); } }
TEST_F(TestInterReflectanceBSDF, TestBSDFInterreflectance) { SCOPED_TRACE("Begin Test: Simple BSDF interreflectance."); CInterReflectance interRefl = *getInterReflectance(); SquareMatrix results = interRefl.value(); const size_t matrixSize = results.size(); // Test matrix size_t size = 7; EXPECT_EQ(size, matrixSize); SquareMatrix correctResults{{1.005964363, 0, 0, 0, 0, 0, 0}, {0, 1.005964363, 0, 0, 0, 0, 0}, {0, 0, 1.006280195, 0, 0, 0, 0}, {0, 0, 0, 1.008724458, 0, 0, 0}, {0, 0, 0, 0, 1.021780268, 0, 0}, {0, 0, 0, 0, 0, 1.176150952, 0}, {0, 0, 0, 0, 0, 0, 3.022280250}}; for(size_t i = 0; i < size; ++i) { for(size_t j = 0; j < size; ++j) { EXPECT_NEAR(correctResults(i, j), results(i, j), 1e-6); } } }
/*---------------------------------------------------------------------------------------------------------------------- | Creates and returns a matrix that represents the inverse of this SquareMatrix. */ SquareMatrix * SquareMatrix::Inverse() const { double * col = new double[dim]; int * permutation = new int[dim]; SquareMatrix * tmp = new SquareMatrix(*this); double ** a = tmp->GetMatrixAsRawPointer(); SquareMatrix * inv = new SquareMatrix(*this); double ** a_inv = inv->GetMatrixAsRawPointer(); // double ** a matrix represented as vector of row pointers // int n order of matrix // double * col work vector of size n // int * permutation work vector of size n // double ** a_inv inverse of input matrix a (matrix a is destroyed) int result = InvertMatrix(a, dim, col, permutation, a_inv); delete tmp; delete [] col; delete [] permutation; if (result != 0) { delete inv; return 0; } return inv; }
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { int* rowStarts = (int*)mxGetData(prhs[0]); int* colIdx = (int*)mxGetData(prhs[1]); double* values = (double*)mxGetData(prhs[2]); int n = (int)*mxGetPr(prhs[3]); int nnz = (int)*mxGetPr(prhs[4]); double *x = (double*)mxGetData(prhs[5]); int *startsOut, *colsOut; double *valOut; double thetta = (double)*mxGetPr(prhs[6]); SquareMatrix* S = new SquareMatrix(n,nnz, rowStarts, colIdx, values); multiplyDiagonalFromLeft(S,x); double* mins = new double[n]; getMinimumsByRow(S, mins); zeroLowerThanThetaMaxInRow(S, mins, thetta); SquareMatrix* St = trimAndTranspose(S); int symNNZ = nnzInSymmetrization(S, St); delete mins; const int dims[] = {1,n}; const int dimsNNZ[] = {1,symNNZ}; plhs[0] = mxCreateNumericArray(2,dims,mxINT32_CLASS,mxREAL); plhs[1] = mxCreateNumericArray(2,dimsNNZ,mxINT32_CLASS,mxREAL); plhs[2] = mxCreateDoubleMatrix(1, symNNZ, mxREAL); startsOut = (int*)mxGetData(plhs[0]); colsOut = (int*)mxGetData(plhs[1]); valOut = mxGetPr(plhs[2]); symmetrisizeAndTrim(S, St, colsOut, valOut, startsOut, n, symNNZ); delete S; St->freeArrays(); delete St; }
Scalar IsotropicLinearElasticity<Scalar,Dim>::energy(const SquareMatrix<Scalar,Dim> &F) const { SquareMatrix<Scalar,Dim> e = 0.5*(F.transpose()+F)-SquareMatrix<Scalar,Dim>::identityMatrix(); Scalar trace_e = e.trace(); Scalar lambda = this->lambda_; Scalar mu = this->mu_; Scalar energy = 0.5*lambda*trace_e*trace_e+mu*e.doubleContraction(e); return energy; }
SquareMatrix<Scalar,Dim> NeoHookean<Scalar,Dim>::secondPiolaKirchhoffStress(const SquareMatrix<Scalar,Dim> &F) const { SquareMatrix<Scalar,Dim> identity = SquareMatrix<Scalar,Dim>::identityMatrix(); SquareMatrix<Scalar,Dim> inverse_c = (F.transpose()*F).inverse(); Scalar lnJ = log(F.determinant()); Scalar mu = this->mu_; Scalar lambda = this->lambda_; SquareMatrix<Scalar,Dim> S = mu*(identity-inverse_c)+lambda*lnJ*inverse_c; return S; }
Scalar NeoHookean<Scalar,Dim>::energy(const SquareMatrix<Scalar,Dim> &F) const { Scalar trace_c = (F.transpose()*F).trace(); Scalar J = F.determinant(); Scalar lnJ = log(J); Scalar mu = this->mu_; Scalar lambda = this->lambda_; Scalar energy = mu/2*(trace_c-Dim)-mu*lnJ+lambda/2*lnJ*lnJ; return energy; }
SquareMatrix<Scalar,Dim> IsotropicLinearElasticity<Scalar,Dim>::cauchyStress(const SquareMatrix<Scalar,Dim> &F) const { SquareMatrix<Scalar,Dim> identity = SquareMatrix<Scalar,Dim>::identityMatrix(); SquareMatrix<Scalar,Dim> e = 0.5*(F.transpose()+F)-identity; Scalar trace_e = e.trace(); Scalar lambda = this->lambda_; Scalar mu = this->mu_; SquareMatrix<Scalar,Dim> stress = lambda*trace_e*identity+2*mu*e; return stress; }
void BlockLocalPositionEstimator::landCorrect() { // measure land Vector<float, n_y_land> y; if (landMeasure(y) != OK) { return; } // measurement matrix Matrix<float, n_y_land, n_x> C; C.setZero(); // y = -(z - tz) C(Y_land_vx, X_vx) = 1; C(Y_land_vy, X_vy) = 1; C(Y_land_agl, X_z) = -1; // measured altitude, negative down dir. C(Y_land_agl, X_tz) = 1; // measured altitude, negative down dir. // use parameter covariance SquareMatrix<float, n_y_land> R; R.setZero(); R(Y_land_vx, Y_land_vx) = _land_vxy_stddev.get() * _land_vxy_stddev.get(); R(Y_land_vy, Y_land_vy) = _land_vxy_stddev.get() * _land_vxy_stddev.get(); R(Y_land_agl, Y_land_agl) = _land_z_stddev.get() * _land_z_stddev.get(); // residual Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * _P * C.transpose()) + R); Vector<float, n_y_land> r = y - C * _x; _pub_innov.get().hagl_innov = r(Y_land_agl); _pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl); // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); // artifically increase beta threshhold to prevent fault during landing float beta_thresh = 1e2f; if (beta / BETA_TABLE[n_y_land] > beta_thresh) { if (!(_sensorFault & SENSOR_LAND)) { _sensorFault |= SENSOR_LAND; mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta)); } // abort correction return; } else if (_sensorFault & SENSOR_LAND) { _sensorFault &= ~SENSOR_LAND; mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK"); } // kalman filter correction always for land detector Matrix<float, n_x, n_y_land> K = _P * C.transpose() * S_I; Vector<float, n_x> dx = K * r; _x += dx; _P -= K * C * _P; }
SquareMatrix<Scalar,Dim> IsotropicLinearElasticity<Scalar,Dim>::firstPiolaKirchhoffStressDifferential( const SquareMatrix<Scalar,Dim> &F, const SquareMatrix<Scalar,Dim> &F_differential) const { Scalar mu = this->mu_; Scalar lambda = this->lambda_; SquareMatrix<Scalar,Dim> identity = SquareMatrix<Scalar,Dim>::identityMatrix(); SquareMatrix<Scalar,Dim> e = 0.5*(F.transpose()+F)-identity; SquareMatrix<Scalar,Dim> e_differential = 0.5*(F_differential.transpose()+F_differential); return 2.0*mu*e_differential+lambda*e_differential.trace()*identity; }
SquareMatrix SquareMatrix::operator+(const SquareMatrix & rhs)const { if (getSize() != rhs.getSize()) throw "Matrix must be equal size!!!"; SquareMatrix result(*this); for (unsigned int i = 0; i < result.getSize(); i++) for (unsigned int j = 0; j < result.getSize(); j++) result[i][j] += rhs.get(i, j); return result; }
void ValidateIdentity(const AffineTransform & t) { SquareMatrix m = t.GetMatrix(); for (unsigned int row = 0; row < m.getRows(); row++) { for (unsigned int col = 0; col < m.getCols(); col++) { if (row == col) { ASSERT_NEAR(1, m[row][col], ABS_ERR); } else { ASSERT_NEAR(0, m[row][col], ABS_ERR); } } } }
SquareMatrix pow(SquareMatrix m, int n) { SquareMatrix res; res.init(); for (; n > 0; n = n >> 1) { if (n & 1) { res = res * m; } m = m * m; } return res; }
double det(const SquareMatrix &a, double EPS = 1e-10) { SquareMatrix b(a); int n = a.size(); double res = 1.0; std::vector<bool> used(n, false); for (int i = 0; i < n; i++) { int p; for (p = 0; p < n; p++) { if (!used[p] && fabs(b[p][i]) > EPS) { break; } } if (p >= n) { return 0; } res *= b[p][i]; used[p] = true; double z = 1.0/b[p][i]; for (int j = 0; j < n; j++) { b[p][j] *= z; } for (int j = 0; j < n; j++) { if (j != p) { z = b[j][i]; for (int k = 0; k < n; k++) { b[j][k] -= z*b[p][k]; } } } } return res; }
double det_naive(const SquareMatrix &a) { int n = a.size(); if (n == 1) { return a[0][0]; } if (n == 2) { return a[0][0]*a[1][1] - a[0][1]*a[1][0]; } double res = 0; SquareMatrix temp(n - 1, typename SquareMatrix::value_type(n - 1)); for (int p = 0; p < n; p++) { int h = 0, k = 0; for (int i = 1; i < n; i++) { for (int j = 0; j < n; j++) { if (j == p) { continue; } temp[h][k++] = a[i][j]; if (k == n - 1) { h++; k = 0; } } } res += (p % 2 == 0 ? 1 : -1)*a[0][p]*det_naive(temp); } return res; }
SquareMatrix SquareMatrix::operator*(const SquareMatrix & matrix) const { if (getSize() != matrix.getSize()) throw "Matrix must be equal size!!!"; SquareMatrix result(getSize()); for (unsigned int i = 0; i < result.getSize(); i++) for (unsigned int j = 0; j < result.getSize(); j++) { double temp = 0; for (unsigned int k = 0; k < getSize(); k++) temp += this->get(i, k) * matrix.get(k, j); result[i][j] = temp; } return result; }
bool operator==(const SquareMatrix &a, const SquareMatrix &b) { if (a.dimensions() != b.dimensions()) { return false; } size_t elementCount = a.elementCount(); for (size_t i = 0; i < elementCount; ++i) { if (a.at(i) != b.at(i)) { return false; } } return true; }
Foam::scalar Foam::det(SquareMatrix<Type>& matrix) { labelList pivotIndices(matrix.n()); label sign; LUDecompose(matrix, pivotIndices, sign); return detDecomposed(matrix, sign); }
/*---------------------------------------------------------------------------------------------------------------------- | Returns the Cholesky decomposition of this SquareMatrix as a lower-triangular matrix. Assumes this SquareMatrix is | symmetric and positive definite. */ SquareMatrix * SquareMatrix::CholeskyDecomposition() const { PHYCAS_ASSERT(dim > 0); SquareMatrix * L = new SquareMatrix(*this); double * p = new double[dim]; double ** a = L->GetMatrixAsRawPointer(); for (unsigned i = 0; i < dim; ++i) { for (unsigned j = i; j < dim; ++j) { double sum = a[i][j]; for (int k = i-1; k >= 0; --k) { sum -= a[i][k]*a[j][k]; } if (i == j) { if (sum <= 0.0) { // matrix is not positive definite return 0; } p[i] = sqrt(sum); } else { a[j][i] = sum/p[i]; } } } // zero out upper diagonal and copy diagonal elements for (unsigned i = 0; i < dim; ++i) { a[i][i] = p[i]; for (unsigned j = i+1; j < dim; ++j) { a[i][j] = 0.0; } } return L; }
Foam::scalar Foam::det(const SquareMatrix<Type>& matrix) { SquareMatrix<Type> matrixTmp = matrix; labelList pivotIndices(matrix.n()); label sign; LUDecompose(matrixTmp, pivotIndices, sign); return detDecomposed(matrixTmp, sign); }
/*---------------------------------------------------------------------------------------------------------------------- | Creates and returns a matrix that represents this matrix raised to the power `p'. */ SquareMatrix * SquareMatrix::Power( double p) const /**< the power to which this matrix should be raised */ { PHYCAS_ASSERT(dim > 0); //PHYCAS_ASSERT(p > 0.0); double * fv = new double[dim]; double * w = new double[dim]; double ** a = this->GetMatrixAsRawPointer(); SquareMatrix * Z = new SquareMatrix(dim, 0.0); double ** z = Z->GetMatrixAsRawPointer(); // Calculate eigenvalues (w) and eigenvectors (z) // int n input: the order of the matrix a // double **a input: the real symmetric matrix // double *fv input: temporary storage array of at least n elements // double **z output: the eigenvectors // double *w output: the eigenvalues in ascending order int err_code = EigenRealSymmetric(dim, a, w, z, fv); if (err_code != 0) { delete Z; delete [] fv; delete [] w; return 0; } SquareMatrix * Zinv = Z->Inverse(); SquareMatrix * Lp = new SquareMatrix(dim, 0.0); // create diagonal matrix of scaled eigenvalues for (unsigned i = 0; i < dim; ++i) { double v = pow(w[i],p); Lp->SetElement(i, i, v); } // Zinv * A * Z = L // A = Z * L * Zinv // A^p = Z * L^p * Zinv SquareMatrix * ZLp = Z->RightMultiplyMatrix(*Lp); SquareMatrix * Ap = ZLp->RightMultiplyMatrix(*Zinv); delete Z; delete Zinv; delete Lp; delete ZLp; delete [] fv; delete [] w; return Ap; }
/*---------------------------------------------------------------------------------------------------------------------- | Creates and returns a matrix that represents the product of this matrix with `matrixOnRight'. */ SquareMatrix * SquareMatrix::RightMultiplyMatrix(SquareMatrix & matrixOnRight) const { double ** l = GetMatrixAsRawPointer(); double ** r = matrixOnRight.GetMatrixAsRawPointer(); SquareMatrix * tmp = new SquareMatrix(*this); double ** t = tmp->GetMatrixAsRawPointer(); for (unsigned i = 0; i < dim; ++i) { for (unsigned j = 0; j < dim; ++j) { double vsum = 0.0; for (unsigned k = 0; k < dim; ++k) { vsum += l[i][k]*r[k][j]; } t[i][j] = vsum; } } return tmp; }
RealType NgammaT::calcConservedQuantity(){ thermostat = snap->getThermostat(); loadEta(); // We need NkBT a lot, so just set it here: This is the RAW number // of integrableObjects, so no subtraction or addition of constraints or // orientational degrees of freedom: NkBT = info_->getNGlobalIntegrableObjects()*PhysicalConstants::kB *targetTemp; // fkBT is used because the thermostat operates on more degrees of freedom // than the barostat (when there are particles with orientational degrees // of freedom). fkBT = info_->getNdf()*PhysicalConstants::kB *targetTemp; RealType totalEnergy = thermo.getTotalEnergy(); RealType thermostat_kinetic = fkBT * tt2 * thermostat.first * thermostat.first /(2.0 * PhysicalConstants::energyConvert); RealType thermostat_potential = fkBT* thermostat.second / PhysicalConstants::energyConvert; SquareMatrix<RealType, 3> tmp = eta.transpose() * eta; RealType trEta = tmp.trace(); RealType barostat_kinetic = NkBT * tb2 * trEta /(2.0 * PhysicalConstants::energyConvert); RealType barostat_potential = (targetPressure * thermo.getVolume() / PhysicalConstants::pressureConvert) /PhysicalConstants::energyConvert; Mat3x3d hmat = snap->getHmat(); RealType area = hmat(0,0) * hmat(1, 1); RealType conservedQuantity = totalEnergy + thermostat_kinetic + thermostat_potential + barostat_kinetic + barostat_potential - surfaceTension * area/ PhysicalConstants::energyConvert; return conservedQuantity; }
SquareMatrix SquareMatrix::operator*(const SquareMatrix &other) const { if (other.columns() != rows()) { throw std::logic_error("SquareMatrix::operator*(): incompatible dimensions"); } SquareMatrix m(mDimensions, false); for (size_t r = 0; r < rows(); ++r) { for (size_t c = 0; c < columns(); ++c) { int &v = m.at(r, c) = 0; for (size_t s = 0; s < rows(); ++s) { v += at(r, s) * other.at(s, c); } } } return m; }
/*---------------------------------------------------------------------------------------------------------------------- | Returns the LU decomposition of this SquareMatrix. */ SquareMatrix * SquareMatrix::LUDecomposition() const { PHYCAS_ASSERT(dim > 0); SquareMatrix * L = new SquareMatrix(*this); double * scaling = new double[dim]; int * permutation = new int[dim]; double ** a = L->GetMatrixAsRawPointer(); int err_code = LUDecompose(a, dim, scaling, permutation, NULL); if (err_code == 0) { // LUDecompose worked delete [] scaling; delete [] permutation; return L; } // Should throw an exception here delete L; delete [] scaling; delete [] permutation; return 0; }
/*---------------------------------------------------------------------------------------------------------------------- | Returns the natural logarithm of the product of the eigenvalues of this SquareMatrix. */ double SquareMatrix::LogDeterminant() const { PHYCAS_ASSERT(dim > 0); double * fv = new double[dim]; double * w = new double[dim]; double ** a = this->GetMatrixAsRawPointer(); SquareMatrix * Z = new SquareMatrix(dim, 0.0); double ** z = Z->GetMatrixAsRawPointer(); // Calculate eigenvalues (w) and eigenvectors (z) // int n input: the order of the matrix a // double **a input: the real symmetric matrix // double *fv input: temporary storage array of at least n elements // double **z output: the eigenvectors // double *w output: the eigenvalues in ascending order int err_code = EigenRealSymmetric(dim, a, w, z, fv); if (err_code != 0) { delete Z; delete [] fv; delete [] w; return 0; } double log_det = 0.0; for (unsigned i = 0; i < dim; ++i) { double v = w[i]; log_det += log(v); } delete Z; delete [] fv; delete [] w; return log_det; }
void EqualMatrices(const SquareMatrix & l, const SquareMatrix & r) { EXPECT_EQ(l.getRows(), r.getRows()); EXPECT_EQ(l.getCols(), r.getCols()); for (unsigned int row = 0; row < l.getRows(); row++) { for (unsigned int col = 0; col < l.getCols(); col++) { ASSERT_NEAR(l[row][col], r[row][col], ABS_ERR); } } }
Foam::scalar Foam::detDecomposed ( const SquareMatrix<Type>& matrix, const label sign ) { scalar diagProduct = 1.0; for (label i = 0; i < matrix.n(); ++i) { diagProduct *= matrix[i][i]; } return sign*diagProduct; }
int main() { LL tmp_a[SIZE][SIZE] = {{1, 4, 1, 0, 1}, {1, 0, 0, 0, 0}, {0, 2, 1, 0, 0}, {0, 1, 0, 0, 1}, {0, 0, 0, 1, 0}}; while (scanf("%d %d", &N, &M) && N != 0) { if (N == 1) { ans = 1; } else { A.init(tmp_a); A = pow(A, N - 2); ans = (A.a[0][0] * 5 + A.a[0][1] * 1 + A.a[0][2] * 2 + A.a[0][3] * 1 + A.a[0][4] * 0) % M; } printf("%d\n", ans); } return 0; }
void BlockLocalPositionEstimator::gpsCorrect() { // measure Vector<double, n_y_gps> y_global; if (gpsMeasure(y_global) != OK) { return; } // gps measurement in local frame double lat = y_global(Y_gps_x); double lon = y_global(Y_gps_y); float alt = y_global(Y_gps_z); float px = 0; float py = 0; float pz = -(alt - _gpsAltOrigin); map_projection_project(&_map_ref, lat, lon, &px, &py); Vector<float, n_y_gps> y; y.setZero(); y(Y_gps_x) = px; y(Y_gps_y) = py; y(Y_gps_z) = pz; y(Y_gps_vx) = y_global(Y_gps_vx); y(Y_gps_vy) = y_global(Y_gps_vy); y(Y_gps_vz) = y_global(Y_gps_vz); // gps measurement matrix, measures position and velocity Matrix<float, n_y_gps, n_x> C; C.setZero(); C(Y_gps_x, X_x) = 1; C(Y_gps_y, X_y) = 1; C(Y_gps_z, X_z) = 1; C(Y_gps_vx, X_vx) = 1; C(Y_gps_vy, X_vy) = 1; C(Y_gps_vz, X_vz) = 1; // gps covariance matrix SquareMatrix<float, n_y_gps> R; R.setZero(); // default to parameter, use gps cov if provided float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get(); float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get(); float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get(); float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get(); // if field is not below minimum, set it to the value provided if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) { var_xy = _sub_gps.get().eph * _sub_gps.get().eph; } if (_sub_gps.get().epv > _param_lpe_gps_z.get()) { var_z = _sub_gps.get().epv * _sub_gps.get().epv; } float gps_s_stddev = _sub_gps.get().s_variance_m_s; if (gps_s_stddev > _param_lpe_gps_vxy.get()) { var_vxy = gps_s_stddev * gps_s_stddev; } if (gps_s_stddev > _param_lpe_gps_vz.get()) { var_vz = gps_s_stddev * gps_s_stddev; } R(0, 0) = var_xy; R(1, 1) = var_xy; R(2, 2) = var_z; R(3, 3) = var_vxy; R(4, 4) = var_vxy; R(5, 5) = var_vz; // get delayed x uint8_t i_hist = 0; if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; } Vector<float, n_x> x0 = _xDelay.get(i_hist); // residual Vector<float, n_y_gps> r = y - C * x0; // residual covariance Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R; // publish innovations for (size_t i = 0; i < 6; i++) { _pub_innov.get().vel_pos_innov[i] = r(i); _pub_innov.get().vel_pos_innov_var[i] = S(i, i); } // residual covariance, (inverse) Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S); // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); // artifically increase beta threshhold to prevent fault during landing float beta_thresh = 1e2f; if (beta / BETA_TABLE[n_y_gps] > beta_thresh) { if (!(_sensorFault & SENSOR_GPS)) { mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g", double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)), double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5))); _sensorFault |= SENSOR_GPS; } } else if (_sensorFault & SENSOR_GPS) { _sensorFault &= ~SENSOR_GPS; mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK"); } // kalman filter correction always for GPS Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I; Vector<float, n_x> dx = K * r; _x += dx; _P -= K * C * _P; }