//function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
void BFilterUKF::utMeasurement(fmat X, fvec Wm, fvec Wc, unsigned int n, fmat R)
{
//Unscented Transformation
//Input:
// f: nonlinear map
// X: sigma points
// Wm: weights for mean
// Wc: weights for covraiance
// n: numer of outputs of f
// R: additive covariance
//Output:
// y: transformed mean
// Y: transformed smapling points
// P: transformed covariance
// Y1: transformed deviations
unsigned int L=X.n_cols;
z1=zeros<fvec>(n);
Z1=zeros<fmat>(n,L);
//for k=1:L
for (unsigned int k=0; k < L; ++k)
{
fmat XColK = X.col(k);
Z1.col(k)= process->ffun(&XColK);
z1=z1+Wm(k)*Z1.col(k);
}
//Z2=Z1-x1(:,ones(1,L));
Z2 = Z1;
for (unsigned int j = 0; j < L; ++j)
{
for (unsigned int i = 0; i < x1.n_rows; ++i)
{
Z2(i,j) -= x1(i);
}
}
P2=Z2*Wc.diag()*Z2.t()+R;
}
//function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
void BFilterUKF::utProcess(fmat X,fvec Wm, fvec Wc, unsigned int n, fmat R)
{
//Unscented Transformation
//Input:
// f: nonlinear map
// X: sigma points
// Wm: weights for mean
// Wc: weights for covraiance
// n: numer of outputs of f
// R: additive covariance
//Output:
// y: transformed mean
// Y: transformed smapling points
// P: transformed covariance
// Y1: transformed deviations
unsigned int L=X.n_cols;
x1 = zeros<fvec>(n);
X1 = zeros<fmat>(n,L);
//for k=1:L
for (unsigned int k=0; k < L; ++k)
{
fmat XColK = X.col(k);
X1.col(k)= process->ffun(&XColK);
x1=x1+Wm(k)*X1.col(k);
}
//X2=X1-x1(:,ones(1,L)); // generate duplicates of vector x1
X2 = X1;
for (unsigned int j = 0; j < L; ++j)
{
for (unsigned int i = 0; i < x1.n_rows; ++i)
{
X2(i,j) -= x1(i);
}
}
P1=X2*Wc.diag()*X2.t()+R;
}
