/// \brief Update Kalman filter
void UnscentedKalmanFilter::updateUKF(vnl_vector<double> u, vnl_vector<double> z)
{
// create Sigma points X for the current estimate distribution
vnl_matrix<double> X(N,2*N+1);
sigmas(x_hat, P_hat+Q, sqrt(C), X);
vnl_matrix<double> Dbg;
qDebug() << "X: ";
for( int i = 0; i<6; i++)
{
qDebug() << X(i,0) << " " << X(i,1) << " " << X(i,2) << " " << X(i,3) << " " << X(i,4) << " " << X(i,5) << " "
<< X(i,6) << " " << X(i,7) << " " << X(i,8) << " " << X(i,9) << " " << X(i,10) << " " << X(i,11)
<< " " << X(i,12);
}
// apply unscented transformation for process model
vnl_vector<double> x1(6);
vnl_matrix<double> X1(N,2*N+1), P1(N,N), X2(N,2*N+1);
utf(X, u, x1, X1, P1, X2);
// apply unscented transformation for observation model
vnl_vector<double> z1(M);
vnl_matrix<double> Z1(M,2*M+1), P2(M,M), Z2(M,2*M+1);
utg(X1, z1, Z1, P2, Z2);
// define transformated cross-covariance
vnl_matrix<double> WC(2*N+1,2*N+1,0.0);
WC.set_diagonal(Wc.get_row(0));
vnl_matrix<double> P12 = X2*WC*Z2.transpose();
// perform state update
K = P12*vnl_matrix_inverse<double>(P2);
x_hat = x1 + K*(z-z1);
// perform covariance update
P_hat = P1 - K*P12.transpose();
}
void sleep_apnea::HP_LP_filter (QVector<QVector<double> > &tab_res)
{
int i,j;
//high-pass filter
QVector<double> Z1(tab_res[0].size());
for(i=0;i<Z1.size();i++)Z1[i]=0;
double CUTOFF = 0.01;
double RC = 1.0/(CUTOFF*2*3.14);
double dt = 1.0/1.0;
double alpha = RC/(RC+dt);
for (j=1;j<tab_res[0].size();j++)
Z1[j]=alpha*(Z1[j-1] + tab_res[1][j] - tab_res[1][j-1]);
//low-pass filter
QVector<double> Z2(tab_res[0].size());
for(int i=0;i<Z2.size();i++)Z2[i]=0;
CUTOFF = 0.09;
RC = 1.0/(CUTOFF*2*3.14);
dt = 1.0/1.0;
alpha = dt/(RC+dt);
for (j=1;j<Z1.size();j++)
Z2[j] = Z2[j-1] + (alpha * (Z1[j] - Z2[j-1]));
//wrtiting to output array
for (i=0;i<tab_res[0].size();i++)
tab_res[1][i]=Z2[i];
}
dMatrix clSpline::GetFirstDerivatives(void)
{
dMatrix dummy;
if(!initialised)
{
Initialise();
}
dummy.SetNumberRows(X1.GetNumberRows());
dummy.SetNumberColumns(3);
for(int i=1; i<=X1.GetNumberRows(); i++)
{
dummy.SetElement(i, 1, fabs(X1(i, 1)) < PRECISION ? 0.0 : X1(i, 1));
dummy.SetElement(i, 2, fabs(Y1(i, 1)) < PRECISION ? 0.0 : Y1(i, 1));
dummy.SetElement(i, 3, fabs(Z1(i, 1)) < PRECISION ? 0.0 : Z1(i, 1));
}
return(dummy);
}
inline void
SymmLLA
( T alpha, const DistMatrix<T>& A, const DistMatrix<T>& B,
T beta, DistMatrix<T>& C )
{
#ifndef RELEASE
PushCallStack("internal::SymmLLA");
if( A.Grid() != B.Grid() || B.Grid() != C.Grid() )
throw std::logic_error
("{A,B,C} must be distributed over the same grid");
#endif
const Grid& g = A.Grid();
DistMatrix<T>
BL(g), BR(g),
B0(g), B1(g), B2(g);
DistMatrix<T>
CL(g), CR(g),
C0(g), C1(g), C2(g);
DistMatrix<T,MC,STAR> B1_MC_STAR(g);
DistMatrix<T,VR,STAR> B1_VR_STAR(g);
DistMatrix<T,STAR,MR> B1Trans_STAR_MR(g);
DistMatrix<T> Z1(g);
DistMatrix<T,MC,STAR> Z1_MC_STAR(g);
DistMatrix<T,MR,STAR> Z1_MR_STAR(g);
DistMatrix<T,MR,MC > Z1_MR_MC(g);
B1_MC_STAR.AlignWith( A );
B1_VR_STAR.AlignWith( A );
B1Trans_STAR_MR.AlignWith( A );
Z1_MC_STAR.AlignWith( A );
Z1_MR_STAR.AlignWith( A );
Scale( beta, C );
LockedPartitionRight
( B, BL, BR, 0 );
PartitionRight
( C, CL, CR, 0 );
while( CL.Width() < C.Width() )
{
LockedRepartitionRight
( BL, /**/ BR,
B0, /**/ B1, B2 );
RepartitionRight
( CL, /**/ CR,
C0, /**/ C1, C2 );
Z1.AlignWith( C1 );
Zeros( C1.Height(), C1.Width(), Z1_MC_STAR );
Zeros( C1.Height(), C1.Width(), Z1_MR_STAR );
//--------------------------------------------------------------------//
B1_MC_STAR = B1;
B1_VR_STAR = B1_MC_STAR;
B1Trans_STAR_MR.TransposeFrom( B1_VR_STAR );
LocalSymmetricAccumulateLL
( TRANSPOSE,
alpha, A, B1_MC_STAR, B1Trans_STAR_MR, Z1_MC_STAR, Z1_MR_STAR );
Z1_MR_MC.SumScatterFrom( Z1_MR_STAR );
Z1 = Z1_MR_MC;
Z1.SumScatterUpdate( T(1), Z1_MC_STAR );
Axpy( T(1), Z1, C1 );
//--------------------------------------------------------------------//
Z1.FreeAlignments();
SlideLockedPartitionRight
( BL, /**/ BR,
B0, B1, /**/ B2 );
SlidePartitionRight
( CL, /**/ CR,
C0, C1, /**/ C2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
Inverse( DistMatrix<F>& A )
{
#ifndef RELEASE
PushCallStack("Inverse");
if( A.Height() != A.Width() )
throw std::logic_error("Cannot invert non-square matrices");
#endif
const Grid& g = A.Grid();
DistMatrix<int,VC,STAR> p( g );
LU( A, p );
TriangularInverse( UPPER, NON_UNIT, A );
// Solve inv(A) L = inv(U) for inv(A)
DistMatrix<F> ATL(g), ATR(g),
ABL(g), ABR(g);
DistMatrix<F> A00(g), A01(g), A02(g),
A10(g), A11(g), A12(g),
A20(g), A21(g), A22(g);
DistMatrix<F> A1(g), A2(g);
DistMatrix<F,VC, STAR> A1_VC_STAR(g);
DistMatrix<F,STAR,STAR> L11_STAR_STAR(g);
DistMatrix<F,VR, STAR> L21_VR_STAR(g);
DistMatrix<F,STAR,MR > L21Trans_STAR_MR(g);
DistMatrix<F,MC, STAR> Z1(g);
PartitionUpDiagonal
( A, ATL, ATR,
ABL, ABR, 0 );
while( ABR.Height() < A.Height() )
{
RepartitionUpDiagonal
( ATL, /**/ ATR, A00, A01, /**/ A02,
/**/ A10, A11, /**/ A12,
/*************/ /******************/
ABL, /**/ ABR, A20, A21, /**/ A22 );
View( A1, A, 0, A00.Width(), A.Height(), A01.Width() );
View( A2, A, 0, A00.Width()+A01.Width(), A.Height(), A02.Width() );
L21_VR_STAR.AlignWith( A2 );
L21Trans_STAR_MR.AlignWith( A2 );
Z1.AlignWith( A01 );
Z1.ResizeTo( A.Height(), A01.Width() );
//--------------------------------------------------------------------//
// Copy out L1
L11_STAR_STAR = A11;
L21_VR_STAR = A21;
L21Trans_STAR_MR.TransposeFrom( L21_VR_STAR );
// Zero the strictly lower triangular portion of A1
MakeTrapezoidal( LEFT, UPPER, 0, A11 );
Zero( A21 );
// Perform the lazy update of A1
internal::LocalGemm
( NORMAL, TRANSPOSE,
F(-1), A2, L21Trans_STAR_MR, F(0), Z1 );
A1.SumScatterUpdate( F(1), Z1 );
// Solve against this diagonal block of L11
A1_VC_STAR = A1;
internal::LocalTrsm
( RIGHT, LOWER, NORMAL, UNIT, F(1), L11_STAR_STAR, A1_VC_STAR );
A1 = A1_VC_STAR;
//--------------------------------------------------------------------//
Z1.FreeAlignments();
L21Trans_STAR_MR.FreeAlignments();
L21_VR_STAR.FreeAlignments();
SlidePartitionUpDiagonal
( ATL, /**/ ATR, A00, /**/ A01, A02,
/*************/ /*******************/
/**/ A10, /**/ A11, A12,
ABL, /**/ ABR, A20, /**/ A21, A22 );
}
// inv(A) := inv(A) P
ApplyInverseColumnPivots( A, p );
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
ReformHermitianMatrix
( UpperOrLower uplo,
DistMatrix<R,MC,MR>& A,
const DistMatrix<R,VR,STAR>& w,
const DistMatrix<R,MC,MR>& Z,
const RealFunctor& f )
{
#ifndef RELEASE
PushCallStack("hermitian_function::ReformHermitianMatrix");
#endif
const Grid& g = A.Grid();
DistMatrix<R,MC,MR> ZL(g), ZR(g),
Z0(g), Z1(g), Z2(g);
DistMatrix<R,VR,STAR> wT(g), w0(g),
wB(g), w1(g),
w2(g);
DistMatrix<R,MC, STAR> Z1_MC_STAR(g);
DistMatrix<R,VR, STAR> Z1_VR_STAR(g);
DistMatrix<R,STAR,MR > Z1Trans_STAR_MR(g);
DistMatrix<R,STAR,STAR> w1_STAR_STAR(g);
if( uplo == LOWER )
MakeTrapezoidal( LEFT, UPPER, 1, A );
else
MakeTrapezoidal( LEFT, LOWER, -1, A );
LockedPartitionRight( Z, ZL, ZR, 0 );
LockedPartitionDown
( w, wT,
wB, 0 );
while( ZL.Width() < Z.Width() )
{
LockedRepartitionRight
( ZL, /**/ ZR,
Z0, /**/ Z1, Z2 );
LockedRepartitionDown
( wT, w0,
/**/ /**/
w1,
wB, w2 );
Z1_MC_STAR.AlignWith( A );
Z1_VR_STAR.AlignWith( A );
Z1Trans_STAR_MR.AlignWith( A );
//--------------------------------------------------------------------//
Z1_MC_STAR = Z1;
Z1_VR_STAR = Z1_MC_STAR;
w1_STAR_STAR = w1;
// Scale Z1[VR,* ] with the modified eigenvalues
const int width = Z1_VR_STAR.Width();
const int localHeight = Z1_VR_STAR.LocalHeight();
for( int j=0; j<width; ++j )
{
const R omega = f(w1_STAR_STAR.GetLocalEntry(j,0));
R* buffer = Z1_VR_STAR.LocalBuffer(0,j);
for( int iLocal=0; iLocal<localHeight; ++iLocal )
buffer[iLocal] *= omega;
}
Z1Trans_STAR_MR.TransposeFrom( Z1_VR_STAR );
internal::LocalTrrk( uplo, (R)1, Z1_MC_STAR, Z1Trans_STAR_MR, (R)1, A );
//--------------------------------------------------------------------//
Z1Trans_STAR_MR.FreeAlignments();
Z1_VR_STAR.FreeAlignments();
Z1_MC_STAR.FreeAlignments();
SlideLockedPartitionDown
( wT, w0,
w1,
/**/ /**/
wB, w2 );
SlideLockedPartitionRight
( ZL, /**/ ZR,
Z0, Z1, /**/ Z2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
ReformNormalMatrix
( DistMatrix<Complex<R>,MC,MR >& A,
const DistMatrix<R, VR,STAR>& w,
const DistMatrix<Complex<R>,MC,MR >& Z,
const ComplexFunctor& f )
{
#ifndef RELEASE
PushCallStack("hermitian_function::ReformNormalMatrix");
#endif
const Grid& g = A.Grid();
typedef Complex<R> C;
DistMatrix<C,MC,MR> ZL(g), ZR(g),
Z0(g), Z1(g), Z2(g);
DistMatrix<R,VR,STAR> wT(g), w0(g),
wB(g), w1(g),
w2(g);
DistMatrix<C,MC, STAR> Z1_MC_STAR(g);
DistMatrix<C,VR, STAR> Z1_VR_STAR(g);
DistMatrix<C,STAR,MR > Z1Adj_STAR_MR(g);
DistMatrix<R,STAR,STAR> w1_STAR_STAR(g);
Zero( A );
LockedPartitionRight( Z, ZL, ZR, 0 );
LockedPartitionDown
( w, wT,
wB, 0 );
while( ZL.Width() < Z.Width() )
{
LockedRepartitionRight
( ZL, /**/ ZR,
Z0, /**/ Z1, Z2 );
LockedRepartitionDown
( wT, w0,
/**/ /**/
w1,
wB, w2 );
Z1_MC_STAR.AlignWith( A );
Z1_VR_STAR.AlignWith( A );
Z1Adj_STAR_MR.AlignWith( A );
//--------------------------------------------------------------------//
Z1_MC_STAR = Z1;
Z1_VR_STAR = Z1_MC_STAR;
w1_STAR_STAR = w1;
// Scale Z1[VR,* ] with the modified eigenvalues
const int width = Z1_VR_STAR.Width();
const int localHeight = Z1_VR_STAR.LocalHeight();
for( int j=0; j<width; ++j )
{
const C conjOmega = Conj(f(w1_STAR_STAR.GetLocalEntry(j,0)));
C* buffer = Z1_VR_STAR.LocalBuffer(0,j);
for( int iLocal=0; iLocal<localHeight; ++iLocal )
buffer[iLocal] *= conjOmega;
}
Z1Adj_STAR_MR.AdjointFrom( Z1_VR_STAR );
internal::LocalGemm
( NORMAL, NORMAL, (C)1, Z1_MC_STAR, Z1Adj_STAR_MR, (C)1, A );
//--------------------------------------------------------------------//
Z1Adj_STAR_MR.FreeAlignments();
Z1_VR_STAR.FreeAlignments();
Z1_MC_STAR.FreeAlignments();
SlideLockedPartitionDown
( wT, w0,
w1,
/**/ /**/
wB, w2 );
SlideLockedPartitionRight
( ZL, /**/ ZR,
Z0, Z1, /**/ Z2 );
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
HermitianFromEVD
( UpperOrLower uplo,
DistMatrix<F>& A,
const DistMatrix<BASE(F),VR,STAR>& w,
const DistMatrix<F>& Z )
{
#ifndef RELEASE
CallStackEntry entry("HermitianFromEVD");
#endif
const Grid& g = A.Grid();
typedef BASE(F) R;
DistMatrix<F> ZL(g), ZR(g),
Z0(g), Z1(g), Z2(g);
DistMatrix<R,VR,STAR> wT(g), w0(g),
wB(g), w1(g),
w2(g);
DistMatrix<F,MC, STAR> Z1_MC_STAR(g);
DistMatrix<F,VR, STAR> Z1_VR_STAR(g);
DistMatrix<F,STAR,MR > Z1Adj_STAR_MR(g);
DistMatrix<R,STAR,STAR> w1_STAR_STAR(g);
A.ResizeTo( Z.Height(), Z.Height() );
if( uplo == LOWER )
MakeTrapezoidal( UPPER, A, 1 );
else
MakeTrapezoidal( LOWER, A, -1 );
LockedPartitionRight( Z, ZL, ZR, 0 );
LockedPartitionDown
( w, wT,
wB, 0 );
while( ZL.Width() < Z.Width() )
{
LockedRepartitionRight
( ZL, /**/ ZR,
Z0, /**/ Z1, Z2 );
LockedRepartitionDown
( wT, w0,
/**/ /**/
w1,
wB, w2 );
Z1_MC_STAR.AlignWith( A );
Z1_VR_STAR.AlignWith( A );
Z1Adj_STAR_MR.AlignWith( A );
//--------------------------------------------------------------------//
Z1_MC_STAR = Z1;
Z1_VR_STAR = Z1_MC_STAR;
w1_STAR_STAR = w1;
DiagonalScale( RIGHT, NORMAL, w1_STAR_STAR, Z1_VR_STAR );
Z1Adj_STAR_MR.AdjointFrom( Z1_VR_STAR );
LocalTrrk( uplo, F(1), Z1_MC_STAR, Z1Adj_STAR_MR, F(1), A );
//--------------------------------------------------------------------//
SlideLockedPartitionDown
( wT, w0,
w1,
/**/ /**/
wB, w2 );
SlideLockedPartitionRight
( ZL, /**/ ZR,
Z0, Z1, /**/ Z2 );
}
}
