// dump statistics and weighted average
void dump(std::ostream& os, std::string msg="PRS") throw(Exception)
{
try {
was.setMessage(msg);
os << was << std::endl;
if(ndof > 0) {
// scale covariance
double sig(::sqrt(APV/ndof));
Matrix<double> Cov(was.getCov());
for(size_t i=0; i<Cov.rows(); i++) for(size_t j=i; j<Cov.cols(); j++)
Cov(i,j) = Cov(j,i) = Cov(i,j)*sig;
// print cov as labelled matrix
Namelist NL;
NL += "ECEF_X"; NL += "ECEF_Y"; NL += "ECEF_Z";
LabeledMatrix LM(NL,Cov);
LM.scientific().setprecision(3).setw(14);
os << "Covariance: " << msg << std::endl << LM << std::endl;
os << "APV: " << msg << std::fixed << std::setprecision(3)
<< " sigma = " << sig << " meters with "
<< ndof << " degrees of freedom.";
}
else os << " Not enough data for covariance.";
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
int main (int argc, char **argv){
int const accSigma = 1;
int const t = (argc == 2)?(lexical_cast<int>(argv[1])):(4);
double const deltaT = 1.0;
dmat A(2,2), B(2,2), G(2,1), Q(2,2);
dmat State(2,1), Cov(2,2), Ctrl(2,1);
std::vector<dmat> Result;
A(0,0) = 1;A(0,1) = deltaT;
A(1,0) = 0;A(1,1) = 1;
B(0,0) = 0;B(0,1) = 0;
B(1,0) = 0;B(1,1) = 0;
G(0,0) = deltaT * deltaT * 0.5;
G(1,0) = deltaT;
Q = accSigma * prod(G, trans(G));
State(0,0) = 0;
State(1,0) = 0;
Cov(0,0) = 1;Cov(0,1) = 0;
Cov(1,0) = 0;Cov(1,1) = 1;
Ctrl(0,0) = 0;
Ctrl(1,0) = 0;
KalmanFilter kf = KalmanFilter(&A, &B, &Q, accSigma);
for(int i = 0; i < t; ++i){
string fileName = "KFCorrect(k=" + lexical_cast<string>(i + 1) + ").txt";
ofstream ofs(fileName.c_str());
Result = kf.Correct(&State, &Cov, &Ctrl, &G);
State = Result.at(0);
Cov = Result.at(1);
for(double location = -20.0; location <= 20.0; location += 0.5){
for(double vel = -10.0; vel <= 10.0; vel += 0.5){
dmat Info(2,1);
Info(0,0) = location;
Info(1,0) = vel;
double prob = kf.calcBelief(&Info, &State, &Cov);
ofs << location << "\t" << vel << "\t" << prob << endl;
}
}
}
return 0;
}
// dump statistics and weighted average
void dump(std::ostream& os, std::string msg="") const throw(Exception)
{
try {
os << "Simple statistics on " << msg << std::endl
<< std::fixed << std::setprecision(3);
if(N > 0) {
os << " " << lab[0] << " N: " << S[0].N()
<< std::fixed << std::setprecision(4)
<< " Ave: " << S[0].Average() << " Std: " << S[0].StdDev()
<< " Min: " << S[0].Minimum() << " Max: " << S[0].Maximum()
<< std::endl;
os << " " << lab[1] << " N: " << S[1].N()
<< std::fixed << std::setprecision(4)
<< " Ave: " << S[1].Average() << " Std: " << S[1].StdDev()
<< " Min: " << S[1].Minimum() << " Max: " << S[1].Maximum()
<< std::endl;
os << " " << lab[2] << " N: " << S[2].N()
<< std::fixed << std::setprecision(4)
<< " Ave: " << S[2].Average() << " Std: " << S[2].StdDev()
<< " Min: " << S[2].Minimum() << " Max: " << S[2].Maximum()
<< std::endl;
os << "Weighted average " << msg << std::endl;
Matrix<double> Cov(inverseSVD(sumInfo));
Vector<double> Sol(Cov * sumInfoState);
os << std::setw(14) << std::setprecision(4) << Sol << " " << N;
}
else os << " No data!";
}
catch(Exception& e) { GPSTK_RETHROW(e); }
}
void CMA::Reset()
{
Optimizer::Reset();
Cov.Clear();
for (int i=0;i<dim;i++) Cov(i,i)=1.0;
std::fill(Pc.begin(),Pc.end(),0);
sigma=0.5;
psucc=ptarget;
}
// -------------------------------------------------------------
/// output at each stage ... the user may override
/// if singular is true, State and Cov may or may not be good
virtual void output(int N) throw()
{
int i;
std::ostringstream oss;
if(stage == Unknown) {
LOG(ERROR) << "Kalman stage not defined in output().";
return;
}
// if MU or SU, output the namelist first
// TD make verbose
if(stage == MU || stage == SU) {
oss << (stage==MU ? "KNL" : "KSL") << msg << " "
<< std::fixed << N << " " << std::setprecision(3) << time;
gpstk::Namelist NL = srif.getNames();
for(i=0; i<NL.size(); i++)
oss << std::setw(10) << NL.getName(i);
LOG(INFO) << oss.str();
oss.str("");
}
// output a label
switch(stage) {
case Init: oss << "KIN"; break;
case AD1:
case AD2:
case AD3: oss << "KAD"; break;
case TU: oss << "KTU"; break;
case MU: oss << "KMU"; break;
case SU: oss << "KSU"; break;
default:
case Unknown:
LOG(INFO) << "Kalman stage not defined." << std::endl;
return;
}
oss << msg << " ";
// output the time
oss << std::fixed << N << " " << std::setprecision(3) << time;
// output the state
for(i=0; i<State.size(); i++)
oss << " " << std::setw(9) << State(i);
// output sqrt of diagonal covariance elements
oss << std::scientific << std::setprecision(2);
for(i=0; i<State.size(); i++)
oss << " " << std::setw(10) << (singular ? 0.0 : sqrt(Cov(i,i)));
LOG(INFO) << oss.str();
}
bool rspfSensorModelTuple::getGroundObsEqComponents(
const rspf_int32 img,
const rspf_int32 iter,
const rspfDpt& obs,
const rspfGpt& ptEst,
rspfDpt& resid,
NEWMAT::Matrix& B,
NEWMAT::SymmetricMatrix& W) const
{
if (iter==0)
{
rspfGpt ptObs(obs.samp,obs.line);
theImages[img]->getForwardDeriv(OBS_INIT, ptObs);
}
resid = theImages[img]->getForwardDeriv(EVALUATE, ptEst);
rspfDpt pWRTx = theImages[img]->getForwardDeriv(P_WRT_X, ptEst);
rspfDpt pWRTy = theImages[img]->getForwardDeriv(P_WRT_Y, ptEst);
rspfDpt pWRTz = theImages[img]->getForwardDeriv(P_WRT_Z, ptEst);
NEWMAT::SymmetricMatrix Cov(2);
rspfSensorModel::CovMatStatus covStatus;
covStatus = theImages[img]->getObsCovMat(obs,Cov);
NEWMAT::Matrix Wfull = invert(Cov);
W << Wfull;
B[0][0] = pWRTx.u;
B[1][0] = pWRTx.v;
B[0][1] = pWRTy.u;
B[1][1] = pWRTy.v;
B[0][2] = pWRTz.u;
B[1][2] = pWRTz.v;
if (traceDebug())
{
rspfNotify(rspfNotifyLevel_DEBUG)
<< "DEBUG: getGroundObsEqComponents:"
<< "\n pWRTx: "<<pWRTx
<< "\n pWRTy: "<<pWRTy
<< "\n pWRTz: "<<pWRTz <<std::endl;
}
bool covOK = false;
if (covStatus == rspfSensorModel::COV_FULL)
covOK = true;
return covOK;
}
