void
KalmanFilter1d::Update(const fixed z_abs, const fixed var_z_abs,
const fixed dt)
{
// Some abbreviated constants to make the code line up nicely:
static constexpr fixed F1 = fixed(1);
// Validity checks. TODO: more?
assert(positive(dt));
// Note: math is not optimized by hand. Let the compiler sort it out.
// Predict step.
// Update state estimate.
x_abs_ += x_vel_ * dt;
// Update state covariance. The last term mixes in acceleration noise.
const fixed dt2 = sqr(dt);
const fixed dt3 = dt * dt2;
const fixed dt4 = sqr(dt2);
p_abs_abs_ += Double(dt*p_abs_vel_) + dt2 * p_vel_vel_ + Quarter(var_x_accel_ * dt4);
p_abs_vel_ += dt * p_vel_vel_ + Half(var_x_accel_ * dt3);
p_vel_vel_ += var_x_accel_ * dt2;
// Update step.
const fixed y = z_abs - x_abs_; // Innovation.
const fixed s_inv = F1 / (p_abs_abs_ + var_z_abs); // Innovation precision.
const fixed k_abs = p_abs_abs_*s_inv; // Kalman gain
const fixed k_vel = p_abs_vel_*s_inv;
// Update state estimate.
x_abs_ += k_abs * y;
x_vel_ += k_vel * y;
// Update state covariance.
p_vel_vel_ -= p_abs_vel_*k_vel;
p_abs_vel_ -= p_abs_vel_*k_abs;
p_abs_abs_ -= p_abs_abs_*k_abs;
}
GeoPoint
Middle(const GeoPoint &a, const GeoPoint &b)
{
// TODO: optimize this naive approach
const fixed distance = Distance(a, b);
return IntermediatePoint(a, b, Half(distance));
}
double
GlidePolar::SinkRate(const double V, const double n) const
{
const auto w0 = SinkRate(V);
const auto vl = VbestLD / std::max(Half(VbestLD), V);
return std::max(0.,
w0 + (V / Double(bestLD)) * (n * n - 1) * vl * vl);
}
fixed
GlidePolar::SinkRate(const fixed V, const fixed n) const
{
const fixed w0 = SinkRate(V);
const fixed vl = VbestLD / std::max(Half(VbestLD), V);
return std::max(fixed(0),
w0 + (V / Double(bestLD)) * (sqr(n) - fixed(1)) * sqr(vl));
}
double
IsolineCrossingFinder::solve()
{
const auto sol = find_zero(Half(xmax + xmin));
if (valid(sol)) {
return sol;
} else {
return -1;
}
}
FlatEllipse::FlatEllipse(const FlatPoint &_f1, const FlatPoint &_f2,
const FlatPoint &_ap)
:f1(_f1),f2(_f2),ap(_ap)
{
const FlatLine f12(f1, f2);
p = f12.ave();
theta = f12.angle();
const fixed csq = f12.dsq();
a = (f1.Distance(ap) + f2.Distance(ap));
assert(sqr(a) >= csq);
b = Half(sqrt(sqr(a) - csq));
a = Half(a);
// a.sin(t) = ap.x
// b.cos(t) = ap.y
FlatPoint op = ap;
op.Subtract(p);
op.Rotate(-theta);
theta_initial = Angle::FromXY(op.x * b, op.y * a).AsDelta();
}
int main(int argc,char *argv[])
{
printf("PROCESS HALF RUNNING....\n");
printf("PROCESS HALF PID = %d\n",getpid());
if (argc == 2 ){
int num = atoi(argv[1]);
printf("RESULT = %d\n",Half(num));
}
else if(argc > 2){
char* args[argc];
char buffer[100];
for (int i=1; i <= argc ;i++){
args[i-1] = argv[i];
}
int num = atoi((argv[argc-1]));
snprintf(buffer,sizeof(buffer),"%d",Half(num));
printf("HALF = %d\n",Half(num));
args[argc-2] = buffer;
args[argc-1]= NULL;
execvp(args[0],args);
}
}
void
AirspaceRoute::Synchronise(const Airspaces& master,
const AGeoPoint& origin,
const AGeoPoint& destination)
{
// @todo: also synchronise with AirspaceWarningManager to filter out items that are
// acknowledged.
h_min = std::min(origin.altitude, std::min(destination.altitude, h_min));
h_max = std::max(origin.altitude, std::max(destination.altitude, h_max));
// @todo: have margin for h_max to allow for climb
AirspacePredicateHeightRangeExcludeTwo condition(h_min, h_max, origin, destination);
if (m_airspaces.SynchroniseInRange(master, origin.Middle(destination),
Half(origin.Distance(destination)),
condition))
{
if (m_airspaces.size())
dirty = true;
}
}
fixed
GlidePolar::GetVTakeoff() const
{
return Half(GetVMin());
}
inline fixed
ZeroFinder::tolerance_actual_zero(const fixed x) const
{
return Double(epsilon * fabs(x)) + Half(tolerance);
}
inline fixed
ZeroFinder::find_min_actual(const fixed xstart)
{
fixed x, v, w; // Abscissae, descr. see above
fixed fx; // f(x)
fixed fv; // f(v)
fixed fw; // f(w)
fixed a = xmin;
fixed b = xmax;
bool x_best = true;
assert(positive(tolerance) && b > a);
/* First step - always gold section*/
x = w = v = a + r * (b - a);
fx = fw = fv = f(v);
// Main iteration loop
for (;;) {
// Range over which the minimum is seeked for
const fixed range = b - a;
const fixed middle_range = Half(a + b);
// Actual tolerance
const fixed tol_act = tolerance_actual_min(x);
const fixed double_tol_act = Double(tol_act);
if (fabs(x-middle_range) + Half(range) <= double_tol_act) {
if (!x_best)
// call once more
fx = f(x);
// Acceptable approx. is found
return x;
}
// Step at this iteration
// Obtain the gold section step
fixed new_step = r * (x < middle_range ? b - x : a - x);
// Decide if the interpolation can be tried
// If x and w are distinct
// interpolation may be tried
if (fabs(x - w) >= tol_act) {
// Interpolation step is calculated as p/q;
// division operation is delayed until last moment
fixed p;
fixed q;
const fixed t = (x - w) * (fx - fv);
q = (x - v) * (fx - fw);
p = (x - v) * q - (x - w) * t;
q = Double(q - t);
// q was calculated with the opposite sign;
// make q positive and assign possible minus to p
if (positive(q))
p = -p;
else
q = -q;
// If x+p/q falls in [a,b] not too close to a and b,
// and isn't too large it is accepted
// If p/q is too large then the gold section procedure can
// reduce [a,b] range to more extent
if (fabs(p) < fabs(new_step * q) && p > q * (a - x + double_tol_act)
&& p < q * (b - x - double_tol_act))
new_step = p / q;
}
// Adjust the step to be not less than tolerance
limit_tolerance(new_step, tol_act);
// Obtain the next approximation to min and reduce the enveloping range
{
// Tentative point for the min
const fixed t = x + new_step;
const fixed ft = f(t);
// t is a better approximation
if (ft <= fx) {
// Reduce the range so that t would fall within it
(t < x ? b : a) = x;
// Assign the best approx to x
v = w;
w = x;
x = t;
fv = fw;
fw = fx;
fx = ft;
x_best = false;
} else {
// x remains the better approx
x_best = true;
// Reduce the range enclosing x
(t < x ? a : b) = t;
if ((ft <= fw) || (w == x)) {
v = w;
w = t;
fv = fw;
fw = ft;
} else if ((ft <= fv) || (v == x) || (v == w)) {
v = t;
fv = ft;
}
}
}
}
}
inline fixed
ZeroFinder::find_zero_actual(const fixed xstart)
{
fixed a, b, c; // Abscissae, descr. see above
fixed fa; // f(a)
fixed fb; // f(b)
fixed fc; // f(c)
bool b_best = true; // b is best and last called
c = a = xmin;
fc = fa = f(a);
b = xmax;
fb = f(b);
// Main iteration loop
for (;;) {
// Distance from the last but one to the last approximation
fixed prev_step = b - a;
if (fabs(fc) < fabs(fb)) {
// Swap data for b to be the best approximation
a = b;
b = c;
c = a;
fa = fb;
fb = fc;
fc = fa;
b_best = false;
} else {
b_best = true;
}
// Actual tolerance
const fixed tol_act = tolerance_actual_zero(b);
// Step at this iteration
fixed new_step = Half(c - b);
if (fabs(new_step) <= tol_act || fabs(fb) < sqrt_epsilon) {
if (!b_best)
// call once more
fb = f(b);
// Acceptable approx. is found
return b;
}
// Decide if the interpolation can be tried
// If prev_step was large enough and was in true direction,
// interpolation may be tried
if (fabs(prev_step) >= tol_act && fabs(fa) > fabs(fb)) {
// Interpolation step is calculated in the form p/q;
// division operations is delayed until the last moment
fixed p;
fixed q;
const fixed cb = c - b;
// If we have only two distinct points
// -> linear interpolation can only be applied
if (a == c) {
const fixed t1 = fb / fa;
p = cb * t1;
q = fixed(1) - t1;
} else {
// Quadric inverse interpolation
q = fa / fc;
const fixed t1 = fb / fc;
const fixed t2 = fb / fa;
p = t2 * (cb * q * (q - t1) - (b - a) * (t1 - fixed(1)));
q = (q - fixed(1)) * (t1 - fixed(1)) * (t2 - fixed(1));
}
// p was calculated with the opposite sign;
// make p positive and assign possible minus to q
if (positive(p))
q = -q;
else
p = -p;
// If b+p/q falls in [b,c] and isn't too large it is accepted
// If p/q is too large then the bissection procedure can
// reduce [b,c] range to more extent
if (p < (fixed_threequaters * cb * q - Half(fabs(tol_act * q)))
&& p < fabs(Half(prev_step * q)))
new_step = p / q;
}
// Adjust the step to be not less than tolerance
limit_tolerance(new_step, tol_act);
// Save the previous approx.
a = b;
fa = fb;
// Do step to a new approxim.
b += new_step;
fb = f(b);
// Adjust c for it to have a sign opposite to that of b
if ((positive(fb) && positive(fc)) || (negative(fb) && negative(fc))) {
c = a;
fc = fa;
}
assert(b >= xmin);
assert(b <= xmax);
}
}
void set_shape(ObservationZone::Shape shape) {
if (oz != NULL && shape == oz->GetShape())
return;
delete oz;
oz = NULL;
fixed radius(10000);
switch (shape) {
case ObservationZone::Shape::LINE:
oz = new LineSectorZone(location, 2 * radius);
break;
case ObservationZone::Shape::CYLINDER:
oz = new CylinderZone(location, radius);
break;
case ObservationZone::Shape::MAT_CYLINDER:
oz = CylinderZone::CreateMatCylinderZone(location);
break;
case ObservationZone::Shape::SECTOR:
oz = new SectorZone(location, radius,
Angle::Degrees(0), Angle::Degrees(70));
break;
case ObservationZone::Shape::ANNULAR_SECTOR:
oz = new AnnularSectorZone(location, radius,
Angle::Degrees(0), Angle::Degrees(70),
Half(radius));
break;
case ObservationZone::Shape::FAI_SECTOR:
oz = SymmetricSectorZone::CreateFAISectorZone(location);
break;
case ObservationZone::Shape::CUSTOM_KEYHOLE:
oz = KeyholeZone::CreateCustomKeyholeZone(location, fixed(10000),
Angle::QuarterCircle());
break;
case ObservationZone::Shape::DAEC_KEYHOLE:
oz = KeyholeZone::CreateDAeCKeyholeZone(location);
break;
case ObservationZone::Shape::BGAFIXEDCOURSE:
oz = KeyholeZone::CreateBGAFixedCourseZone(location);
break;
case ObservationZone::Shape::BGAENHANCEDOPTION:
oz = KeyholeZone::CreateBGAEnhancedOptionZone(location);
break;
case ObservationZone::Shape::BGA_START:
oz = KeyholeZone::CreateBGAStartSectorZone(location);
break;
case ObservationZone::Shape::SYMMETRIC_QUADRANT:
oz = new SymmetricSectorZone(location);
break;
}
if (oz != NULL)
oz->SetLegs(&previous, &next);
if (IsDefined())
Invalidate();
}
// add using dual numbers
// combine diffeq and incorporation for direct RedTrace computation
void DerivativeRedTrace(float *red_out, float *ival_offset, float *da_offset, float *dk_offset,
int npts, float *deltaFrameSeconds, float *deltaFrame,
float *nuc_rise_ptr, int SUB_STEPS, int my_start,
Dual A, float SP,
Dual kr, float kmax, float d,
float sens, float gain, float tauB,
PoissonCDFApproxMemo *math_poiss)
{
int i;
Dual totocc, totgen;
// mixed_poisson_struct mix_ctrl;
MixtureMemo mix_memo;
Dual pact,pact_new;
Dual c_dntp_top, c_dntp_bot;
Dual hplus_events_sum, hplus_events_current; // mean events per molecule, cumulative and current
Dual ldt;
Dual Ival;
Dual One(1.0);
Dual Zero(0.0);
Dual Half(0.5);
Dual xSP(SP);
Dual xkmax(kmax);
Dual xd(d);
Dual xA = mix_memo.Generate(A,math_poiss);
//xA.Dump("xA");
//A = InitializeMixture(&mix_ctrl,A,MAX_HPLEN); // initialize Poisson with correct amplitude which maxes out at MAX_HPLEN
mix_memo.ScaleMixture(SP);
//ScaleMixture(&mix_ctrl,SP); // scale mixture fractions to proper number of molecules
pact = Dual(mix_memo.total_live); // active polymerases // wrong???
//pact.Dump("pact");
//Dual pact_zero = pact;
totocc = xSP*xA; // how many hydrogens we'll eventually generate
//totocc.Dump("totocc");
totgen = totocc; // number remaining to generate
//totgen.Dump("totgen");
c_dntp_bot = Zero; // concentration of dNTP in the well
c_dntp_top = Zero; // concentration at top
hplus_events_sum = hplus_events_current = Zero; // Events per molecule
memset(ival_offset,0,sizeof(float[my_start])); // zero the points we don't compute
memset(da_offset,0,sizeof(float[my_start])); // zero the points we don't compute
memset(dk_offset,0,sizeof(float[my_start])); // zero the points we don't compute
Dual scaled_kr = kr*Dual(n_to_uM_conv)/xd; // convert molecules of polymerase to active concentraction
Dual half_kr = kr *Half; // for averaging
// kr.Dump("kr");
//scaled_kr.Dump("scaled_kr");
//half_kr.Dump("half_kr");
// first non-zero index of the computed [dNTP] array for this nucleotide
int c_dntp_top_ndx = my_start*SUB_STEPS;
Dual c_dntp_bot_plus_kmax = Dual(1.0/kmax);
Dual c_dntp_old_effect = Zero;
Dual c_dntp_new_effect = Zero;
Dual cur_gen(0.0);
Dual equilibrium(0.0);
int st;
// trace variables
Dual old_val = Zero;
Dual cur_val = Zero;
Dual run_sum = Zero;
Dual half_dt = Zero;
Dual TauB(tauB);
Dual SENS(sens);
memset(red_out,0,sizeof(float[my_start]));
for (i=my_start;i < npts;i++)
{
// Do one prediction time step
if (totgen.a > 0.0)
{
// need to calculate incorporation
ldt = (deltaFrameSeconds[i]/SUB_STEPS); // multiply by half_kr out here, because I'm going to use it twice?
ldt *= half_kr; // scale time by rate out here
//ldt.Dump("ldt");
for (st=1; (st <= SUB_STEPS) && (totgen.a > 0.0);st++)
{
c_dntp_bot.a = nuc_rise_ptr[c_dntp_top_ndx];
c_dntp_top_ndx++;
// calculate denominator
equilibrium = c_dntp_bot_plus_kmax;
equilibrium *= pact;
equilibrium *= scaled_kr;
equilibrium += One;
c_dntp_bot /= equilibrium;
//c_dntp_bot.Dump("c_dntp_bot");
// the level at which new nucs are used up as fast as they diffuse in
c_dntp_bot_plus_kmax.Reciprocal(c_dntp_bot + xkmax); // scale for michaelis-menten kinetics, assuming nucs are limiting factor
//c_dntp_bot_plus_kmax.Dump("plus_kmax");
// Now compute effect of concentration on enzyme rate
c_dntp_old_effect = c_dntp_new_effect;
c_dntp_new_effect = c_dntp_bot;
c_dntp_new_effect *= c_dntp_bot_plus_kmax; // current effect of concentration on enzyme rate
//c_dntp_new_effect.Dump("c_dntp_new");
// update events per molecule
hplus_events_current = c_dntp_old_effect;
hplus_events_current += c_dntp_new_effect;
hplus_events_current *= ldt;
//hplus_events_current.Dump("current"); // events per molecule is average rate * time of rate
hplus_events_sum += hplus_events_current;
//hplus_events_sum.Dump("sum");
// how many active molecules left at end of time period given poisson process with total intensity of events
// exp(-t) * (1+t+t^2/+t^3/6+...) where we interpolate between polynomial lengths by A
// exp(-t) ( 1+... + frac*(t^k/k!)) where k = ceil(A-1) and frac = A-floor(A), for A>=1
pact_new = mix_memo.GetStep(hplus_events_sum);
//pact_new = pact_zero;
//pact_new.Dump("pact_new");
// how many hplus were generated
// reuse pact for average
// reuse hplus-events_current for total events
pact += pact_new;
pact *= Half; // average number of molecules
//hplus_events_current *= pact; // events/molecule *= molecule is total events
totgen -= pact*hplus_events_current; // active molecules * events per molecule
//totgen.Dump("totgen");
pact = pact_new; // update to current number of molecules
//pact.Dump("pact");
}
if (totgen.a < 0.0) totgen = Zero;
}
Ival = totocc;
Ival -= totgen;
ival_offset[i] = Ival.a;
Ival *= SENS; // convert from hydrogen to counts
// Now compute trace
// trace
half_dt = deltaFrame[i]*0.5;
// calculate new value
Ival *= TauB;
cur_val = Ival;
cur_val -= run_sum;
old_val *= half_dt; // update
cur_val -= old_val;
cur_val /= (TauB+half_dt);
// update run sum
run_sum += old_val; // reuse update
run_sum += cur_val*half_dt;
old_val = cur_val; // set for next value
//cur_val *= gain; // gain=1.0 always currently
red_out[i] = cur_val.a;
da_offset[i] = cur_val.da;
dk_offset[i] = cur_val.dk;
// now we have done one prediction time step
}
}
fixed GetMiddleX() const {
return Half(x_min + x_max);
}
/**
* After take-off has been detected, we check if the ground speed goes
* below a certain threshold that indicates the aircraft has ceased
* flying. To avoid false positives while wave/ridge soaring, this
* threshold is half of the given take-off speed.
*/
gcc_pure
static bool
CheckLandingSpeed(fixed takeoff_speed, const NMEAInfo &basic)
{
return !CheckTakeOffSpeed(Half(takeoff_speed), basic);
}
CirclingWind::Result
CirclingWind::CalcWind()
{
assert(circle_count > 0);
assert(!samples.empty());
// reject if average time step greater than 2.0 seconds
if ((samples.back().time - samples[0].time) / (samples.size() - 1) > 2)
return Result(0);
// find average
double av = 0;
for (unsigned i = 0; i < samples.size(); i++)
av += samples[i].vector.norm;
av /= samples.size();
// find zero time for times above average
double rthismax = 0;
double rthismin = 0;
int jmax = -1;
int jmin = -1;
for (unsigned j = 0; j < samples.size(); j++) {
double rthisp = 0;
for (unsigned i = 1; i < samples.size(); i++) {
const unsigned ithis = (i + j) % samples.size();
unsigned idiff = i;
if (idiff > samples.size() / 2)
idiff = samples.size() - idiff;
rthisp += samples[ithis].vector.norm * idiff;
}
if ((rthisp < rthismax) || (jmax == -1)) {
rthismax = rthisp;
jmax = j;
}
if ((rthisp > rthismin) || (jmin == -1)) {
rthismin = rthisp;
jmin = j;
}
}
// attempt to fit cycloid
const auto mag = Half(samples[jmax].vector.norm - samples[jmin].vector.norm);
if (mag >= 30)
// limit to reasonable values (60 knots), reject otherwise
return Result(0);
double rthis = 0;
for (const Sample &sample : samples) {
const auto sc = sample.vector.bearing.SinCos();
auto wx = sc.second, wy = sc.first;
wx = wx * av + mag;
wy *= av;
auto cmag = hypot(wx, wy) - sample.vector.norm;
rthis += Square(cmag);
}
rthis /= samples.size();
rthis = sqrt(rthis);
int quality;
if (mag > 1)
quality = 5 - iround(rthis / mag * 3);
else
quality = 5 - iround(rthis);
if (circle_count < 3)
quality--;
if (circle_count < 2)
quality--;
if (quality < 1)
//measurment quality too low
return Result(0);
/* 5 is maximum quality, make sure we honour that */
quality = std::min(quality, 5);
// jmax is the point where most wind samples are below
SpeedVector wind(samples[jmax].vector.bearing.Reciprocal(), mag);
return Result(quality, wind);
}
