private void KalmanFilterButton_Click(object sender, RoutedEventArgs e)
{
if (CountriesListView.SelectedItem == null)
{
return;
}
PWTCountry country = (CountriesListView.SelectedItem as CountryVM).CountryObject;
SortedDictionary <int, double> kalmanDataSet = country.SavingsRateHT;
int minYear = kalmanDataSet.Keys.Min();
int maxYear = kalmanDataSet.Keys.Max();
if (!double.TryParse(TimeStepTerm.Text, out double dt) || dt < 0)
{
MessageBox.Show("invalid time step");
return;
}
int numXVals = (int)((maxYear - minYear) / dt) + 1;
// time steps
MathMatrix t = Sequences.SteppedSequence(minYear, maxYear, dt);
// state matrix
MathMatrix xt = MathMatrix.CreateMatrix(3, numXVals, 0);
double[] timeArray = t.RowVectorArray(0);
for (int colidx = 0; colidx < t.ColCount; ++colidx)
{
if (Math.Floor(timeArray[colidx]) == Math.Ceiling(timeArray[colidx]) && country.AGrowthRateHT.ContainsKey((int)timeArray[colidx]))
{
int key = (int)timeArray[colidx];
xt[0, colidx] = country.AGrowthRateHT[key];
xt[1, colidx] = country.SavingsRateHT[key];
xt[2, colidx] = country.LGrowthRateHT[key];
}
else if (colidx > 0)
{
xt[0, colidx] = xt[0, colidx - 1];
xt[1, colidx] = xt[1, colidx - 1];
xt[2, colidx] = xt[2, colidx - 1];
}
}
// state matrix
// x(k) = Fx(k-1) + Gu(k-1) which can be seen below in Kalman filter loop.
MathMatrix x = MathMatrix.CreateMatrix(3, numXVals, 0);
// process matrix moves state matrix from state k to k + 1
MathMatrix F = MatrixOperations.Identity(3);
MathMatrix FT = F;
// control matrix
MathMatrix u = MathMatrix.CreateMatrix(1, 1, 0);
MathMatrix G = MathMatrix.CreateMatrix(3, 1, new double[] { 0, 0, 0 });
// state error covariance matrix
if (!double.TryParse(CovarianceTerm.Text, out double covterm))
{
MessageBox.Show("Covariance term needs to be a double value.");
return;
}
//MathMatrix P = MathMatrix.CreateMatrix(3, 3, new double[] { -0.1, 0.05, -0.1, 0.001, 0.01, -0.005, -0.005, 0.15, -0.05 });
MathMatrix P = MathMatrix.CreateMatrix(3, 3, new double[] { covterm, 0, 0, 0, covterm, 0, 0, 0, covterm });
// observation matrix
MathMatrix H = MathMatrix.CreateMatrix(1, 3, new double[] { 1, 1, 1 });
MathMatrix HT = MatrixOperations.Transpose(H);
// process noise covariance matrix
MathMatrix Q = MathMatrix.CreateMatrix(3, 3, new double[] { -0.01, 0.05, -0.1, 0.001, -0.01, -0.005, -0.005, 0.15, -0.05 });
MathMatrix I = MatrixOperations.Identity(3);
// measurement noise covariance matrix
MathMatrix R = MathMatrix.CreateMatrix(1, 1, 3);
MathMatrix sqrtR = MatrixOperations.Sqrt_Elmtwise(R);
// measurement noise
MathMatrix v = sqrtR * Distributions.Normal(numXVals);
// observation / measurement
// y(k) = Hxt(k) + v(k);
MathMatrix y = H * xt + v;
// Kalman filter
for (int k = 0; k < numXVals; ++k)
{
x.AssignColumn(F * xt.ColumnVector(k) + G * u, k);
P = F * P * FT + Q;
// HACK HERE SINCE WE DO NOT YET HAVE MATRIX INVERSION.
MathMatrix Knumerator = P * HT;
MathMatrix Kdenominator = (H * P * HT + R);
Kdenominator[0, 0] = 1 / Kdenominator[0, 0];
MathMatrix K = Knumerator * Kdenominator;
x.AssignColumn(x.ColumnVector(k) + K * (y.ColumnVector(k) - H * x.ColumnVector(k)), k);
P = (I - K * H) * P;
}
ResultsPlot.ClearPlotArea(clearPlotData: true);
int kalmanrowidx = KalmanFilterPlotType == "n" ? 2 : KalmanFilterPlotType == "s" ? 1 : KalmanFilterPlotType == "g" ? 0 : -1;
if (kalmanrowidx == -1)
{
MessageBox.Show("A filter quantity must be selected.");
return;
}
// GO THROUGH THE ARRAYS AND DROP CORRESPONDING NAN OR INFINITY ENTRIES FROM X AND XT.
double[] numsOnlyVec = x.RowVectorArray(kalmanrowidx).Where(p => !double.IsNaN(p) && !double.IsInfinity(p)).ToArray();
double minY = new double[] { numsOnlyVec.Min(), xt.RowVectorArray(kalmanrowidx).Min() }.Min();
double maxY = new double[] { numsOnlyVec.Max(), xt.RowVectorArray(kalmanrowidx).Max() }.Max();
ap.YLabel = YAxisLabel.NewAxisLabel(KalmanFilterPlotQtyDict[KalmanFilterPlotType], 0.5, 15, ylp);
ap.XLabel = XAxisLabel.NewAxisLabel("Year", minY < 0 ? 0.05 : 0.5, 15, minY < 0 ? xlp2 : xlp);
ResultsPlot.SetAxes(minYear, maxYear, minY, maxY, ap, drawHorAxisAtY0: minY < 0);
ResultsPlot.SetPlotGridLines(20, 20);
PointCollection pc = new PointCollection();
PointCollection pc2 = new PointCollection();
for (int idx = 0; idx < numsOnlyVec.Length; ++idx)
{
pc.Add(new Point(t[0, idx], numsOnlyVec[idx]));
pc2.Add(new Point(t[0, idx], xt[kalmanrowidx, idx]));
}
ResultsPlot.PlotPoints2D($"KalmanFiltered_{country.CountryCode}_{kalmanrowidx}_Points", pc, dpp);
ResultsPlot.PlotCurve2D($"KalmanFiltered_{country.CountryCode}_{kalmanrowidx}_Curve", pc2, cp2);
}
private void EstimatePosition_Click(object sender, RoutedEventArgs e)
{
double dt = 0.1;
int N = 100;
MathMatrix t = Sequences.SteppedSequence(0, N * dt, dt);
MathMatrix F = MathMatrix.CreateMatrix(2, 2, new double[] { 1, dt, 0, 1 });
MathMatrix FT = MatrixOperations.Transpose(F);
MathMatrix G = MathMatrix.CreateMatrix(2, 1, new double[] { 0.5 * dt * dt, dt });
MathMatrix H = MathMatrix.CreateMatrix(1, 2, new double[] { 1, 0 });
MathMatrix HT = MatrixOperations.Transpose(H);
MathMatrix Q = MathMatrix.CreateMatrix(2, 2, new double[] { 0, 0, 0, 0 });
MathMatrix u = MathMatrix.CreateMatrix(1, 1, -9.80665);
MathMatrix I = MatrixOperations.Identity(2);
double y0 = 100;
double v0 = 0;
MathMatrix xt = MathMatrix.CreateMatrix(2, N, 0);
xt[0, 0] = y0;
xt[1, 0] = v0;
MathMatrix x = MathMatrix.CreateMatrix(2, N, 0);
x[0, 0] = 105;
if (double.TryParse(Y0.Text, out double qt))
{
xt[0, 0] = qt;
x[0, 0] = qt;
}
qt = 0;
x[1, 0] = qt;
if (double.TryParse(V0.Text, out qt))
{
xt[1, 0] = qt;
x[1, 0] = qt;
}
for (int k = 1; k < N; ++k)
{
xt.AssignColumn(F * xt.ColumnVector(k - 1) + G * u, k);
}
MathMatrix R = MathMatrix.CreateMatrix(1, 1, 4);
MathMatrix sqrtR = MatrixOperations.Sqrt_Elmtwise(R);
MathMatrix v = sqrtR * Distributions.Normal(N);
MathMatrix z = H * xt + v;
MathMatrix P = MathMatrix.CreateMatrix(2, 2, new double[] { 10, 0, 0, 0.1 });
for (int k = 1; k < N; ++k)
{
x.AssignColumn(F * x.ColumnVector(k - 1) + G * u, k);
P = F * P * FT + Q;
// HACK HERE SINCE WE DO NOT YET HAVE MATRIX INVERSION.
MathMatrix Knumerator = P * HT;
MathMatrix Kdenominator = (H * P * HT + R);
Kdenominator[0, 0] = 1 / Kdenominator[0, 0];
MathMatrix K = Knumerator * Kdenominator;
x.AssignColumn(x.ColumnVector(k) + K * (z.ColumnVector(k) - H * x.ColumnVector(k)), k);
P = (I - K * H) * P;
}
ResultsPlot.ClearPlotArea();
DataPointPreferences dpp = DataPointPreferences.CreateObject(Colors.Black, 1, 4, 4);
AxesPreferences2D ap = AxesPreferences2D.CreateObject(Colors.Red, Colors.Blue, 1, 1, 40, 1);
ResultsPlot.SetAxes(0, 10, -400, 110, ap);
PointCollection pc = new PointCollection();
for (int idx = 0; idx < x.ColCount; ++idx)
{
pc.Add(new Point(t[0, idx], x[0, idx]));
}
ResultsPlot.PlotCurve2D("test", pc);
ResultsPlot.PlotPoints2D("test", pc, dpp);
}