public MatrixKalmanWrapper()
{
/*
* X0 : predicted state
* P0 : predicted covariance
*
* F : factor of real value to previous real value
* B : the control-input model which is applied to the control vector uk;
* U : the control-input model which is applied to the control vector uk;
* Q : measurement noise
* H : factor of measured value to real value
* R : environment noise
*/
var f = new Matrix(new[, ] {
{ 1.0, 1 }, { 0, 1.0 }
});
var b = new Matrix(new[, ] {
{ 0.0 }, { 0 }
});
var u = new Matrix(new[, ] {
{ 0.0 }, { 0 }
});
var r = Matrix.CreateVector(10);
var q = new Matrix(new[, ] {
{ 0.01, 0.4 }, { 0.1, 0.02 }
});
var h = new Matrix(new[, ] {
{ 1.0, 0 }
});
kX = makeKalmanFilter(f, b, u, q, h, r);
kY = makeKalmanFilter(f, b, u, q, h, r);
kZ = makeKalmanFilter(f, b, u, q, h, r);
}
public MatrixKalmanWrapper(double set_q)
{
var Ts = 0.02;
var Q = set_q;
var G = Matrix.CreateVector(Mathf.Pow((float)Ts, 3) / 6, Mathf.Pow((float)Ts, 3) / 2, Ts);
var a = new Matrix(new[, ] {
{ 0, 1.0, 0 }, { 0, 0, 1.0 }, { 0, 0, 0 }
}); //a
/*
* X0 : predicted state
* P0 : predicted covariance
*
* F : factor of real value to previous real value
* B : the control-input model which is applied to the control vector uk;
* U : the control-input model which is applied to the control vector uk;
* Q : measurement noise
* H : factor of measured value to real value
* R : environment noise
*/
var f = Matrix.IdentityMatrix(3) + (Ts * a) + 0.5 * Matrix.Power(Ts * a, 2); //ad
var b = Matrix.CreateVector(0);
var u = Matrix.CreateVector(0);
var r_good = Matrix.CreateVector(0.68);
var r_bad = Matrix.CreateVector(1000000000000);
var q = (Q * G) * G.Transpose(); //g * q * g.T
var h = new Matrix(new[, ] {
{ 1.0, 0, 0 }
}); //c
kY = makeKalmanFilter(f, b, u, q, h, r_good, r_bad);
}
private void WindowLoaded(object sender, RoutedEventArgs e)
{
var temp = new PlotModel();
var rs = new LineSeries();
rs.StrokeThickness = 1;
var fuelData = File.ReadAllLines(@"Data/Fuel.txt").Select(double.Parse).ToList();
for (int i = 0; i < fuelData.Count; i++) rs.Points.Add(new DataPoint(i, fuelData[i]));
temp.Series.Add(rs);
var f = new Matrix(new[,] {{1.0, 1}, {0, 1}});
var b = new Matrix(new[,] {{0.0}, {0}});
var u = new Matrix(new[,] {{0.0}, {0}});
var r = Matrix.CreateVector(10);
var q = new Matrix(new[,] {{0.01, 0.4}, {0.1, 0.02}});
var h = new Matrix(new[,] {{1.0 , 0}});
var kalman = new KalmanFilter(f, b, u, q, h, r); // задаем F, H, Q и R
kalman.SetState(Matrix.CreateVector(fuelData[0], 0), new Matrix(new[,] {{10.0, 0}, {0 , 10.0}})); // задаем начальные State и Covariance
var filtered = new List<double>();
var rashod = new List<double>();
foreach (var d in fuelData)
{
try
{
kalman.Correct(new Matrix(new[,] {{d}}));
filtered.Add(kalman.State[0, 0]);
rashod.Add(kalman.State[1, 0]);
}
catch(Exception ex)
{
MessageBox.Show(ex.Message);
}
}
var fs = new LineSeries();
for (int i = 0; i < filtered.Count; i++) fs.Points.Add(new DataPoint(i, filtered[i]));
temp.Series.Add(fs);
var ras = new LineSeries();
for (int i = 0; i < filtered.Count; i++) ras.Points.Add(new DataPoint(i, rashod[i]));
temp.Series.Add(ras);
temp.Axes.Add(new LinearAxis(AxisPosition.Left, 0, 4200));
temp.Axes.Add(new LinearAxis(AxisPosition.Bottom));
Plot.Model = temp;
}
KalmanFilter makeKalmanFilter(Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r)
{
var filter = new KalmanFilter (
f.Duplicate (),
b.Duplicate (),
u.Duplicate (),
q.Duplicate (),
h.Duplicate (),
r.Duplicate ()
);
// set initial value
filter.SetState (
Matrix.CreateVector (500, 0),
new Matrix (new [,] {{10.0, 0}, {0, 5.0}})
);
return filter;
}
public MatrixKalmanWrapper()
{
/*
* X0 : predicted state
* P0 : predicted covariance
*
* F : factor of real value to previous real value
* B : the control-input model which is applied to the control vector uk;
* U : the control-input model which is applied to the control vector uk;
* Q : measurement noise
* H : factor of measured value to real value
* R : environment noise
*/
var ts = 0.033;
var f = new Matrix(new[, ] {
{ 1.0, ts, 0 }, { 0, 1, 0 }, { 0, 0, 1 }
});
var b = new Matrix(new[, ] {
{ 0.0 }, { 0 }
});
var u = new Matrix(new[, ] {
{ 0.0 }, { 0 }
});
//var r = Matrix.CreateVector (10);
//var q = new Matrix (new[,] {{0.01, 0.4}, {0.1, 0.02}});
var h = new Matrix(new[, ] {
{ 1.0, 0, 0 }, { 1, 0, 0 }, { 0, 1, -1 }, { 0, 1, -1 }
});
var r = new Matrix(new[, ] {
{ 1.0, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 }
});
var gd = new Matrix(new[, ] {
{ 1 / 2 * Mathf.Pow((float)ts, 2), 0 }, { ts, 0 }, { 0, ts }
});
var q_raw = new Matrix(new[, ] {
{ 100, 0 }, { 0, 10.0 }
});
var q = gd * q_raw * gd.Transpose();
kX = makeKalmanFilter(f, b, u, q, h, r);
kY = makeKalmanFilter(f, b, u, q, h, r);
kZ = makeKalmanFilter(f, b, u, q, h, r);
}
KalmanFilter makeKalmanFilter(Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r)
{
var filter = new KalmanFilter(
f.Duplicate(),
b.Duplicate(),
u.Duplicate(),
q.Duplicate(),
h.Duplicate(),
r.Duplicate()
);
// set initial value
filter.SetState(
Matrix.CreateVector(500, 0),
new Matrix(new [, ] {
{ 10.0, 0 }, { 0, 5.0 }
})
);
return(filter);
}
public MatrixKalmanWrapper()
{
/*
X0 : predicted state
P0 : predicted covariance
F : factor of real value to previous real value
B : the control-input model which is applied to the control vector uk;
U : the control-input model which is applied to the control vector uk;
Q : measurement noise
H : factor of measured value to real value
R : environment noise
*/
var f = new Matrix (new[,] {{1.0, 1}, {0, 1.0}});
var b = new Matrix (new[,] {{0.0}, {0}});
var u = new Matrix (new[,] {{0.0}, {0}});
var r = Matrix.CreateVector (10);
var q = new Matrix (new[,] {{0.01, 0.4}, {0.1, 0.02}});
var h = new Matrix (new[,] {{1.0 , 0}});
kX = makeKalmanFilter (f, b, u, q, h, r);
kY = makeKalmanFilter (f, b, u, q, h, r);
kZ = makeKalmanFilter (f, b, u, q, h, r);
}
private void WindowLoaded(object sender, RoutedEventArgs e)
{
var temp = new PlotModel();
var rs = new LineSeries();
rs.StrokeThickness = 1;
var fuelData = File.ReadAllLines(@"Data/Fuel.txt").Select(double.Parse).ToList();
for (int i = 0; i < fuelData.Count; i++)
{
rs.Points.Add(new DataPoint(i, fuelData[i]));
}
temp.Series.Add(rs);
var f = new Matrix(new[, ] {
{ 1.0, 1 }, { 0, 1 }
});
var b = new Matrix(new[, ] {
{ 0.0 }, { 0 }
});
var u = new Matrix(new[, ] {
{ 0.0 }, { 0 }
});
var r = Matrix.CreateVector(10);
var q = new Matrix(new[, ] {
{ 0.01, 0.4 }, { 0.1, 0.02 }
});
var h = new Matrix(new[, ] {
{ 1.0, 0 }
});
var kalman = new KalmanFilter(f, b, u, q, h, r); // задаем F, H, Q и R
kalman.SetState(Matrix.CreateVector(fuelData[0], 0), new Matrix(new[, ] {
{ 10.0, 0 }, { 0, 10.0 }
})); // задаем начальные State и Covariance
var filtered = new List <double>();
var rashod = new List <double>();
foreach (var d in fuelData)
{
try
{
kalman.Correct(new Matrix(new[, ] {
{ d }
}));
filtered.Add(kalman.State[0, 0]);
rashod.Add(kalman.State[1, 0]);
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
}
var fs = new LineSeries();
for (int i = 0; i < filtered.Count; i++)
{
fs.Points.Add(new DataPoint(i, filtered[i]));
}
temp.Series.Add(fs);
var ras = new LineSeries();
for (int i = 0; i < filtered.Count; i++)
{
ras.Points.Add(new DataPoint(i, rashod[i]));
}
temp.Series.Add(ras);
temp.Axes.Add(new LinearAxis(AxisPosition.Left, 0, 4200));
temp.Axes.Add(new LinearAxis(AxisPosition.Bottom));
Plot.Model = temp;
}