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;
}
void key_SetOldYearZone(void)
{
if (bKey == bKEY_ENTER)
{
if (enKeyboard == KBD_ENTER)
{
if (GetZoneKeySize() == 0)
{
NeedProgram(bSET_DAY_ZONE);
return;
}
if (fPublicTrf == true)
{
switch (wProgram)
{
case bSET_PROGRAM10: LoadSlide(pszPubPrg10);
ibXmax = 12;
ShowOldZones(); break;
case bSET_PROGRAM17: LoadSlide(pszPubPrg17);
ibXmax = 4;
ShowOldZones(); break;
case bSET_PROGRAM20:
case bSET_PROGRAM27: NeedProgram(bSET_PUBLIC_TARIFFS);
return;
}
}
else
{
switch (wProgram)
{
case bSET_PROGRAM10: LoadSlide(pszPowPrg10);
ibXmax = 12;
ShowOldZones(); break;
case bSET_PROGRAM17: LoadSlide(pszPowPrg17);
ibXmax = 4;
ShowOldZones(); break;
case bSET_PROGRAM20: LoadSlide(pszEngPrg20);
ibXmax = 12;
ShowOldZones(); break;
case bSET_PROGRAM27: LoadSlide(pszEngPrg27);
ibXmax = 4;
ShowOldZones(); break;
}
}
enKeyboard = KBD_INPUT1;
}
else if (enKeyboard == KBD_POSTINPUT1)
{
if ((ibX = GetCharLo(10,11)) == 0)
{
enKeyboard = KBD_POSTENTER;
ibYmin = 0;
ibYmax = 11;
pe = YEAR;
}
else if ((ibXmax == 4) && (ibX <= ibXmax))
{
enKeyboard = KBD_POSTENTER;
switch (ibX)
{
case 1: ibYmin = 0; ibYmax = 2; break;
case 2: ibYmin = 3; ibYmax = 5; break;
case 3: ibYmin = 6; ibYmax = 8; break;
case 4: ibYmin = 9; ibYmax = 11; break;
}
pe = QUARTER;
}
else if ((ibXmax == 12) && (ibX <= ibXmax))
{
enKeyboard = KBD_POSTENTER;
ibYmin = ibX-1;
ibYmax = ibX-1;
pe = MONTH;
}
else Beep();
}
if (enKeyboard == KBD_POSTENTER)
{
if (fPublicTrf == true)
{
switch (wProgram)
{
case bSET_PROGRAM10:
case bSET_PROGRAM17:
SetPeriodTariffsPow(ibYmin,ibYmax,&zoKey,pe);
SetPeriodTariffsEng(ibYmin,ibYmax,&zoKey,pe);
ShowOldZones(); break;
}
}
else
{
switch (wProgram)
{
case bSET_PROGRAM10:
case bSET_PROGRAM17:
SetPeriodTariffsPow(ibYmin,ibYmax,&zoKey,pe);
ShowOldZones(); break;
case bSET_PROGRAM20:
case bSET_PROGRAM27:
SetPeriodTariffsEng(ibYmin,ibYmax,&zoKey,pe);
ShowOldZones(); break;
}
}
}
}
else if (bKey < 10)
{
if ((enKeyboard == KBD_INPUT1) || (enKeyboard == KBD_POSTENTER))
{
enKeyboard = KBD_INPUT1;
switch (wProgram)
{
case bSET_PROGRAM10:
case bSET_PROGRAM20: Month(); break;
case bSET_PROGRAM17:
case bSET_PROGRAM27: Quarter(); break;
}
}
if ((enKeyboard == KBD_INPUT1) || (enKeyboard == KBD_POSTINPUT1))
{
enKeyboard = KBD_POSTINPUT1;
ShiftLo(10,11);
}
else Beep();
}
else Beep();
}
static constexpr inline fixed LargeMinLeg(fixed total) {
return Quarter(total);
}
