void csCameraBase::Correct (int n)
{
if (n == 0) return;
csVector3 w1, w2, w3;
float *vals[5];
w3 = m_t2o.Col3 ();
vals[0] = &w3.x;
vals[1] = &w3.y;
vals[2] = &w3.z;
vals[4] = 0;
Correct (n, vals); /* perform the snap-to operation on the forward vector */
/* Maybe w3 should be normalized. Only necessary if there is
significant roundoff error: */
// w3 = csVector3::unit(w3);
/* perhaps a snap-to should be performed on one of the other vectors as well */
w1 = m_t2o.Col2 ();
w2 = csVector3::Unit (w3 % w1);
w1 = w2 % w3;
SetT2O (csMatrix3 (w1.x, w2.x, w3.x, w1.y, w2.y, w3.y, w1.z, w2.z, w3.z));
}
// Compute the a posteriori estimate of the system state, as well as
// the a posteriori estimate error variance. Version for
// one-dimensional systems without control input on the system.
//
// @param phiValue State transition gain.
// @param processNoiseVariance Process noise variance.
// @param measurement Measurement value.
// @param measurementsGain Measurements gain.
// @param measurementsNoiseVariance Measurements noise variance.
//
// @return
// 0 if OK
// -1 if problems arose
//
int SimpleKalmanFilter::Compute( const double& phiValue,
const double& processNoiseVariance,
const double& measurement,
const double& measurementsGain,
const double& measurementsNoiseVariance )
throw(InvalidSolver)
{
try
{
Predict( phiValue,
xhat(0),
processNoiseVariance );
Correct( measurement,
measurementsGain,
measurementsNoiseVariance );
}
catch(InvalidSolver e)
{
GPSTK_THROW(e);
return -1;
}
return 0;
} // End of method 'SimpleKalmanFilter::Compute()'
void csCameraBase::Correct (int n, float *vals[])
{
if (vals == 0) return;
if (vals[0] == 0) return;
if (vals[1] == 0) return;
float r;
if (vals[2] != 0)
{
if (*vals[0] < *vals[1])
{
r = *vals[2];
*vals[2] = *vals[0];
*vals[0] = r;
}
else
{
r = *vals[2];
*vals[2] = *vals[1];
*vals[1] = r;
}
}
float angle;
angle = (float)atan2 (*vals[1], *vals[0]);
angle = (TWO_PI / n) * csQround (n * angle / TWO_PI);
*vals[1] = csQsqrt ((*vals[0]) * (*vals[0]) + (*vals[1]) * (*vals[1]));
Correct (n, vals + 1);
r = *vals[1];
*vals[0] = r * (float)cos (angle);
*vals[1] = r * (float)sin (angle);
cameranr = cur_cameranr++;
}
// Compute the a posteriori estimate of the system state, as well as
// the a posteriori estimate error covariance matrix. This version
// assumes that no control inputs act on the system.
//
// @param phiMatrix State transition matrix.
// @param processNoiseCovariance Process noise covariance matrix.
// @param measurements Measurements vector.
// @param measurementsMatrix Measurements matrix. Called geometry
// matrix in GNSS.
// @param measurementsNoiseCovariance Measurements noise covariance
// matrix.
//
// @return
// 0 if OK
// -1 if problems arose
//
int SimpleKalmanFilter::Compute( const Matrix<double>& phiMatrix,
const Matrix<double>& processNoiseCovariance,
const Vector<double>& measurements,
const Matrix<double>& measurementsMatrix,
const Matrix<double>& measurementsNoiseCovariance )
throw(InvalidSolver)
{
try
{
Predict( phiMatrix,
xhat,
processNoiseCovariance );
Correct( measurements,
measurementsMatrix,
measurementsNoiseCovariance );
}
catch(InvalidSolver e)
{
GPSTK_THROW(e);
return -1;
}
return 0;
} // End of method 'SimpleKalmanFilter::Compute()'
void main()
{
boolean b=Correct("()()()()())",15);
if(b)
cout<<"合法"<<endl;
else
cout<<"不合法"<<endl;
}
/**
\brief Run an iteration of the tracker loop.
Predict and correct, adjusting precision and stepsize as necessary.
\return Success if the step was successful, and a non-success code if something went wrong, such as a linear algebra failure or AMP Criterion violation.
*/
SuccessCode TrackerIteration() const override
{
static_assert(std::is_same< typename Eigen::NumTraits<RT>::Real,
typename Eigen::NumTraits<CT>::Real>::value,
"underlying complex type and the type for comparisons must match");
this->NotifyObservers(NewStep<EmitterType >(*this));
Vec<CT>& predicted_space = std::get<Vec<CT> >(this->temporary_space_); // this will be populated in the Predict step
Vec<CT>& current_space = std::get<Vec<CT> >(this->current_space_); // the thing we ultimately wish to update
CT current_time = CT(this->current_time_);
CT delta_t = CT(this->delta_t_);
SuccessCode predictor_code = Predict(predicted_space, current_space, current_time, delta_t);
if (predictor_code!=SuccessCode::Success)
{
this->NotifyObservers(FirstStepPredictorMatrixSolveFailure<EmitterType >(*this));
this->next_stepsize_ = this->stepping_config_.step_size_fail_factor*this->current_stepsize_;
UpdateStepsize();
return predictor_code;
}
this->NotifyObservers(SuccessfulPredict<EmitterType , CT>(*this, predicted_space));
Vec<CT>& tentative_next_space = std::get<Vec<CT> >(this->tentative_space_); // this will be populated in the Correct step
CT tentative_next_time = current_time + delta_t;
SuccessCode corrector_code = Correct(tentative_next_space,
predicted_space,
tentative_next_time);
if (corrector_code == SuccessCode::GoingToInfinity)
{
// there is no corrective action possible...
return corrector_code;
}
else if (corrector_code!=SuccessCode::Success)
{
this->NotifyObservers(CorrectorMatrixSolveFailure<EmitterType >(*this));
this->next_stepsize_ = this->stepping_config_.step_size_fail_factor*this->current_stepsize_;
UpdateStepsize();
return corrector_code;
}
this->NotifyObservers(SuccessfulCorrect<EmitterType , CT>(*this, tentative_next_space));
// copy the tentative vector into the current space vector;
current_space = tentative_next_space;
return SuccessCode::Success;
}
/** Corrects (or "measurement updates") the a posteriori estimate
* of the system state vector, as well as the a posteriori estimate
* error covariance matrix, using as input the predicted a priori
* state vector and error covariance matrix, plus measurements and
* associated matrices.
*
* @param measurements Measurements vector.
* @param measurementsMatrix Measurements matrix. Called geometry
* matrix in GNSS.
* @param measurementsNoiseCovariance Measurements noise covariance
* matrix.
*
* @return
* 0 if OK
* -1 if problems arose
*/
virtual int MeasUpdate( const Vector<double>& measurements,
const Matrix<double>& measurementsMatrix,
const Matrix<double>& measurementsNoiseCovariance)
throw(InvalidSolver)
{
return Correct(measurements,
measurementsMatrix,
measurementsNoiseCovariance);
}
WebAddress::WebAddress(const String_256& StringToParse, WebCorrectFlags wcfToUse)
{
//First make a copy of the string we've been given
String_256 strCopy=StringToParse;
//And correct the copy
Correct(&strCopy, wcfToUse);
//Then parse the corrected string
Parse(strCopy);
//And finally set the class's flags
SetFlags();
}
char Input::Key(char ascii, KeySym keysym, bool singleInputMode) {
char tmp = 0;
// Take a screenshot
if(keysym == XK_F11) {
system(cfg->getOption("screenshot_cmd").c_str());
}
if (!singleInputMode && (keysym == XK_Tab || keysym == XK_ISO_Left_Tab)) {
if (Field == GET_NAME) {
// Move to next field
Field = GET_PASSWD;
} else {
Field = GET_NAME;
}
} else if(keysym == XK_Return) {
if(!strcmp(NameBuffer, ""))
return tmp;
// Check for special command (console, halt, reboot, exit)
int special = SpecialWanted();
if(Field == GET_NAME) {
// Move to next field
Field = GET_PASSWD;
// Check for special command (console, exit)
if(special == CONSOLE || special == EXIT)
Action = special;
} else {
// Check for special command (halt, reboot)
if(special == REBOOT || special == HALT)
Action = SpecialCorrect(special);
// Regular login
else {
if(Correct())
Action = LOGIN;
else
Action = FAIL;
}
}
} else if(keysym == XK_Delete || keysym == XK_BackSpace)
tmp = DeleteLast();
else if(isprint(ascii))
if(keysym < XK_Shift_L || keysym > XK_Hyper_R)
Add(ascii);
return tmp;
}
void ProcessContigs(ContigStoragePtr storage) {
double processed_perc = 0.1;
double step = 0.1;
for(size_t i = 0; i < storage->Size(); i++) {
storage->ReplaceContig(Correct((*storage)[i]), i);
double cur_process_perc = static_cast<double>(i) / static_cast<double>(storage->Size());
if(cur_process_perc > processed_perc) {
while(processed_perc + step <= cur_process_perc)
processed_perc += step;
INFO(ToString(processed_perc * 100.0) << "% contigs were processed");
processed_perc += step;
}
}
INFO("100% contigs were processed");
}
int Input::SpecialCorrect(int special) {
int result, c;
char tmp[INPUT_MAXLENGTH_NAME];
strcpy(tmp, NameBuffer);
strcpy(NameBuffer, "root");
c = Correct();
strcpy(NameBuffer, tmp);
if(c)
result = special;
else
result = FAIL;
return result;
}
//Function checks if all tiles are in correct position
bool Fifteen::CorrectGameArea()
{
int correct = 0;
for (int i = 0; i < SIZE; i++)
{
for (int j = 0; j < SIZE; j++)
{
if (Correct(i,j))
{
correct++;
}
}
}
return correct >= SIZE*SIZE - 1;
}
SuccessCode Step(config::Predictor predictor_choice,
Vec<ComplexType> & next_space, ComplexType & next_time,
System & sys,
Vec<ComplexType> const& current_space, ComplexType current_time,
ComplexType const& delta_t,
RealType & condition_number_estimate,
unsigned & num_steps_since_last_condition_number_computation,
unsigned frequency_of_CN_estimation, PrecisionType prec_type,
RealType const& tracking_tolerance,
RealType const& path_truncation_threshold,
unsigned min_num_newton_iterations,
unsigned max_num_newton_iterations,
config::AdaptiveMultiplePrecisionConfig const& AMP_config)
{
SuccessCode predictor_code = Predict(next_space,
sys,
current_space, current_time,
delta_t,
condition_number_estimate,
num_steps_since_last_condition_number_computation,
frequency_of_CN_estimation, prec_type,
tracking_tolerance,
AMP_config);
if (predictor_code!=SuccessCode::Success)
return predictor_code;
next_time = current_time + delta_t;
SuccessCode corrector_code = Correct(next_space,
sys,
current_space, // pass by value to get a copy of it
next_time,
prec_type,
tracking_tolerance,
path_truncation_threshold,
min_num_newton_iterations,
max_num_newton_iterations,
AMP_config);
if (corrector_code!=SuccessCode::Success)
return corrector_code;
return SuccessCode::Success;
}
ContigStoragePtr Correct(ContigStoragePtr contigs) {
for(size_t i = 0; i < contigs->Size(); i++)
contigs->ReplaceContig(Correct((*contigs)[i]), i);
TRACE(contigs->Size() << " contigs from " << contigs->Size() << " were corrected");
return contigs;
}
//------------------------------------------------------------------------------
bool PredictorCorrector::Step()
{
#ifdef DEBUG_PROPAGATION
MessageInterface::ShowMessage("Called PredictorCorrector::Step()\n");
#endif
if (!isInitialized)
return false;
// if (physicalModel->StateChanged(true))
// {
// MessageInterface::ShowMessage(" Resetting in Step()\n");
// Reset();
// }
bool wasStartup = false;
do
{
if (!startupComplete)
{
wasStartup = true;
if (ddt == NULL)
{
ddt = physicalModel->GetDerivativeArray();
if (ddt == NULL)
return false;
}
// First load up the derivatives for the current state
physicalModel->GetDerivatives(inState);
memcpy(history[startupCount+1], ddt,
dimension * sizeof(Real));
#ifdef DEBUG_PROPAGATION
MessageInterface::ShowMessage("startup ddt = [%le %le %le...]\n", ddt[0], ddt[1], ddt[2]);
#endif
bool retval = FireStartupStep();
if (retval)
timeleft -= stepTaken;
else
return false;
// Omit any error from the starter code -- we assume it is good
// enough for startup purposes
maxError = 0.0;
}
else
{
#ifdef DEBUG_PROPAGATION
MessageInterface::ShowMessage("Pred-Corr step ");
#endif
++stepAttempts;
if (!Predict())
return false;
if (!Correct())
return false;
if (EstimateError() < 0.0)
return false;
if (maxError <= tolerance)
{
memcpy(outState, correctorState, dimension * sizeof(Real));
physicalModel->IncrementTime(stepSize);
stepTaken = stepSize;
timeleft -= stepTaken;
}
#ifdef DEBUG_PROPAGATION
MessageInterface::ShowMessage("Max error = %le, tolerance = %le\n",
maxError, tolerance);
#endif
if ((maxError > tolerance) || (maxError < lowerError))
if (maxError != 0.0)
if (!AdaptStep(maxError))
return false;
}
if (stepAttempts >= maxStepAttempts)
return false;
} while (maxError > tolerance);
#ifdef DEBUG_PROPAGATION
MessageInterface::ShowMessage("Good step\n");
#endif
if ((startupComplete) && (wasStartup == false))
{
stepAttempts = 0;
}
return true;
}
void Phx::PositionalCorrection()
{
for(int i=0;i<Collisions.size();i++)
Correct(Collisions[i]); // Dokonujemy korekty po³o¿enia
}
auto Correct(int i) {
if (i == 1)
return i; // return type deduced as int
else
return Correct(i-1)+i; // ok to call it now
}
