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;
}
