private void MakeRImpl()
{
_modified = true;
MakeR();
if (_modified)
{
_flags |= KALMAN.R_MODIF;
}
}
private void MakeBaseVImpl()
{
_modified = true;
MakeBaseV();
if (_modified)
{
_flags |= KALMAN.V_MODIF;
}
}
public void TimeUpdateStep(double[] _u)
{
SizeUpdate();
u.Swap <double>(_u);
MakeCommonProcess();
MakeAImpl();
MakeWImpl();
MakeQImpl();
MakeProcess();
if (!_optimizeQ)
{
if (_flags == KALMAN.Q_MODIF)
{
_Q = Q;
Factor(_Q);
UpperInvert(_Q);
}
Q.Swap <double>(_Q);
// W_ = W*U n.nw = n.nw * nw.nw
if ((_flags == KALMAN.W_MODIF) || (_flags == KALMAN.Q_MODIF))
{
for (int i = 0; i < n; ++i)
{
for (int j = 0; j < nw; ++j)
{
_W[i, j] = W[i, j];
for (int k = 0; k < j; ++k)
{
_W[i, j] += W[i, k] * Q[j, k];
}
}
}
}
W.Swap <double>(_W);
}
TimeUpdate();
if (!_optimizeQ)
{
Q.Swap <double>(_Q);
W.Swap <double>(_W);
}
u.Swap <double>(_u);
_flags &= ~KALMAN.MIDMASK;
}
public void Initialize(double[] x, double[,] P)
{
_x.Swap <double>(x);
_P.Swap <double>(P);
_flags |= KALMAN.P_MODIF;
}
protected virtual void SizeUpdate()
{
if (_flags == 0)
{
return;
}
if (_flags == KALMAN.N_MODIF)
{
A = Matrix.Create <double>(n);
MakeBaseAImpl();
}
if ((_flags == KALMAN.N_MODIF) || (_flags == KALMAN.NW_MODIF))
{
_nn = n + nw;
_a = Matrix.Vector <double>(_nn, 0.0);
_v = Matrix.Vector <double>(_nn, 0.0);
_d = Matrix.Vector <double>(_nn, 0.0);
if (!_optimizeQ)
{
_W = Matrix.Create <double>(n, nw);
}
W = Matrix.Create <double>(n, nw);
MakeBaseWImpl();
}
if (_flags == KALMAN.P_MODIF)
{
_U = Matrix.Create <double>(n, _nn);
for (int i = 0; i < n; ++i)
{
for (int j = 0; j < n; ++j)
{
_U[i, j] = _P[i, j];
}
}
Factor(_U);
}
else if (_flags == KALMAN.NW_MODIF)
{
_P = Matrix.Create <double>(n, _nn);
for (int i = 0; i < n; ++i)
{
for (int j = 0; j < n; ++j)
{
_P[i, j] = _U[i, j];
}
}
Factor(_P);
}
if (_flags == KALMAN.NW_MODIF)
{
if (!_optimizeQ)
{
_Q = Matrix.Create <double>(nw);
}
Q = Matrix.Create <double>(nw);
MakeBaseQImpl();
}
if (m != 0)
{
if ((_flags == KALMAN.N_MODIF) || (_flags == KALMAN.M_MODIF))
{
if (!_optimizeVR)
{
_H = Matrix.Create <double>(m, n);
}
H = Matrix.Create <double>(m, n);
MakeBaseHImpl();
}
if ((_flags == KALMAN.M_MODIF) || (_flags == KALMAN.NV_MODIF))
{
V = Matrix.Create <double>(m, nv);
MakeBaseVImpl();
}
if (_flags == KALMAN.NV_MODIF)
{
R = Matrix.Create <double>(nv);
MakeBaseRImpl();
}
if (_flags == KALMAN.M_MODIF)
{
_R = Matrix.Create <double>(m);
z = Matrix.Vector <double>(m, 0.0);
dz = Matrix.Vector <double>(m, 0.0);
}
}
_flags &= ~KALMAN.LOWMASK;
}
public void MeasureUpdateStep(double[] _z)
{
SizeUpdate();
if (m == 0)
{
return;
}
MakeCommonMeasure();
MakeHImpl();
MakeVImpl();
MakeRImpl();
MakeMeasure();
for (int i = 0; i < m; ++i)
{
dz[i] = _z[i] - z[i];
}
MakeDZ();
if (_optimizeVR)
{
if ((_flags == KALMAN.V_MODIF) || (_flags == KALMAN.R_MODIF))
{
for (int i = 0; i < m; ++i)
{
_R[i, i] = V[i, i] * V[i, i] * R[i, i];
}
}
}
else
{
if ((_flags == KALMAN.V_MODIF) || (_flags == KALMAN.R_MODIF))
{
_x = Matrix.Vector <double>(nv, 0.0);
// R_ = V*R*V'
for (int i = 0; i < m; ++i)
{
// _x = row i of V*R = (V*R)(i,:)
for (int j = 0; j < nv; ++j)
{
_x[j] = 0.0;
for (int k = 0; k < nv; ++k)
{
_x[j] += V[i, k] * R[k, j];
}
}
// R_(i,:) = (V*R*V')(i,:) = (V*R)(i,:) * V'
for (int j = 0; j < m; ++j)
{
_R[i, j] = 0.0;
for (int k = 0; k < nv; ++k)
{
_R[i, j] += _x[k] * V[j, k];
}
}
}
// R_ = U*D*U'
// diag(R_) = D, upper(R_) = U, lower(R_) = junk
Factor(_R);
// lower(R_) = (inv(U))'
UpperInvert(_R);
}
if ((_flags == KALMAN.H_MODIF) || (_flags == KALMAN.V_MODIF) || (_flags == KALMAN.R_MODIF))
{
// H_ = inv(U)*H m.n = m.m * m.n
for (int i = 0; i < m; ++i)
{
for (int j = 0; j < n; ++j)
{
_H[i, j] = H[i, j];
for (int k = i + 1; k < m; ++k)
{
_H[i, j] += _R[k, i] * H[k, j];
}
}
}
}
H.Swap <double>(_H);
// _x = inv(U)*dz m.1 = m.m * m.1
_x = Matrix.Vector <double>(m, 0.0);
for (int i = 0; i < m; ++i)
{
_x[i] = dz[i];
for (int k = i + 1; k < m; ++k)
{
_x[i] += _R[k, i] * dz[k];
}
}
dz.Swap <double>(_x);
}
_x = Matrix.Vector <double>(n, 0.0); // dx : innovation
for (int i = 0; i < m; ++i)
{
for (int j = 0; j < n; ++j)
{
_a[j] = H[i, j];
}
MeasureUpdate(dz[i], _R[i, i]);
}
for (int i = 0; i < n; ++i)
{
x[i] += _x[i];
}
if (!_optimizeVR)
{
H.Swap <double>(_H);
}
_flags &= ~KALMAN.HIGHMASK;
}
