public Vector3 Update(Vector3 current)
{
kX.Correct(new Matrix(new double[, ] {
{ current.x }
}));
kY.Correct(new Matrix(new double[, ] {
{ current.y }
}));
kZ.Correct(new Matrix(new double[, ] {
{ current.z }
}));
// rashod
// kX.State [1,0];
// kY.State [1,0];
// kZ.State [1,0];
Vector3 filtered = new Vector3(
(float)kX.State [0, 0],
(float)kY.State [0, 0],
(float)kZ.State [0, 0]
);
return(filtered);
}
public Vector3 Update(Vector3 current)
{
kX.Correct(new Matrix(new double[, ] {
{ current.x }
}));
kY.Correct(new Matrix(new double[, ] {
{ current.y }
}));
kZ.Correct(new Matrix(new double[, ] {
{ current.z }
}));
// rashod
// kX.State [1,0];
// kY.State [1,0];
// kZ.State [1,0];
Vector3 filtered = new Vector3(
(float)kX.State [0, 0],
(float)kY.State [0, 0],
(float)kZ.State [0, 0]
);
Vector3 v = new Vector3(
(float)kX.State [1, 0],
(float)kY.State [1, 0],
(float)kZ.State [1, 0]
);
Debug.Log("Kalman values: position" + filtered.ToString() + ", velocity " + v.ToString());
return(filtered);
}
public Vector3 Update(Vector4 current_phi, Vector4 current_gyro)
{
kX.Correct(new Matrix(new double[, ] {
{ current_phi.w }, { current_phi.x }, { current_gyro.w }, { current_gyro.x }
}));
kY.Correct(new Matrix(new double[, ] {
{ current_phi.y }, { current_phi.z }, { current_gyro.y }, { current_gyro.z }
}));
//kZ.Correct (new Matrix (new double[,] {{current.z}}));
// rashod
// kX.State [1,0];
// kY.State [1,0];
// kZ.State [1,0];
Vector3 filtered = new Vector3(
(float)kX.State [0, 0],
(float)kY.State [0, 0],
(float)kZ.State [0, 0]
);
return(filtered);
}
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;
}
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;
}
