public MainWindowViewModel()
{
Measurements = new ObservableCollection <Measurement>();
Estimates = new ObservableCollection <Measurement>();
var filter = new UKF();
var N = 100;
for (int k = 1; k < N; k++)
{
double[] z = ProcessBuilder.SineWave(k);
filter.Update(z);
var state = filter.getState();
var covariance = filter.getCovariance();
Measurements.Add(new Measurement()
{
Value = z[0], Time = TimeSpan.FromSeconds(k)
});
Estimates.Add(new Measurement()
{
Value = state[0], Time = TimeSpan.FromSeconds(k), Variance = covariance[0, 0]
});
}
}
static void Main(string[] args)
{
var filter = new UKF(1, 1);
var n = 1; //number of state
var q = 0.05; //std of process
var r = 0.3; //std of measurement
var Q = Matrix.Build.Diagonal(n, n, q * q); //covariance of process
var R = Matrix.Build.Dense(1, 1, r * r); //covariance of measurement
var f = new FEquation(); //nonlinear state equations
var h = new HEquation(); //measurement equation
var x = q * Matrix.Build.Random(1, 1); //s + q * Matrix.Build.Random(1, 1); //initial state with noise
var P = Matrix.Build.Diagonal(n, n, 1); //initial state covraiance
var N = 100; //total dynamic steps
var xV = Matrix.Build.Dense(n, N, 0); //estmate
var zV = Matrix.Build.Dense(1, N, 0); //measurement
for (int k = 1; k < N; k++)
{
var z = Matrix.Build.Dense(1, 1, Math.Sin(k * 3.14 * 5 / 180)).Add(Matrix.Build.Random(1, 1).Multiply(r)); //measurments
zV.SetSubMatrix(0, k, z); //save measurment
var x_and_P = filter.Update(f, x, P, h, z, Q, R); //ukf
x = x_and_P[0];
P = x_and_P[1];
xV.SetColumn(k, x.Column(0).ToArray()); //save estimate
}
GraphPane myPane = new GraphPane(new RectangleF(0, 0, 3200, 2400), "Unscented Kalman Filter", "number", "measurement");
PointPairList list_zV = new PointPairList();
PointPairList list_xV = new PointPairList();
for (int i = 0; i < zV.ColumnCount; i++)
{
list_zV.Add(i, zV[0, i]);
list_xV.Add(i, xV[0, i]);
}
myPane.AddCurve("measurement", list_zV, Color.Red, SymbolType.Circle);
myPane.AddCurve("estimate", list_xV, Color.Green, SymbolType.XCross);
Bitmap bm = new Bitmap(200, 200);
Graphics g = Graphics.FromImage(bm);
myPane.AxisChange(g);
Image im = myPane.Image;
im.Save("result.png", ImageFormat.Png);
}
/// <summary>
/// UnscentedKalmanFilter 对一维数据进行处理
/// </summary>
/// <param name="data">使用引用传值方式修改data</param>
public static void UnscentedKalmanFilterMethod(List <double> data)
{
var filter = new UKF();
for (int i = 0; i < data.Count; i++)
{
filter.Update(new[] { data[i] });
// filter.Update(new[] { data1[i], data2[i] });
//filter.Update(new[] { data1[i], data2[i], data3[i] });
try
{
data[i] = filter.getState()[0];
}
catch
{
System.Diagnostics.Debug.WriteLine("UnscentedKalmanFilterMethod:filter index 超出");
}
}
}
public void Run()
{
var filter = new UKF(1, 1);
n = 1;
q = 0.05;
r = 0.3;
N = 100;
Q = Matrix.Build.Diagonal(n, n, q * q); //covariance of process
NotifyChanged("Q");
R = Matrix.Build.Dense(1, 1, r * r); //covariance of measurement
f = new FEquation(); //nonlinear state equations
h = new HEquation(); //measurement equation
x = q * Matrix.Build.Random(1, 1); //s + q * Matrix.Build.Random(1, 1); //initial state with noise
P = Matrix.Build.Diagonal(n, n, 1); //initial state covariance
var xV = Matrix.Build.Dense(n, N, 0); //Estimate
var zV = Matrix.Build.Dense(1, N, 0); //measurement
for (int k = 1; k < N; k++)
{
Matrix <double> z = ProcessBuilder.SineWave(k, r);
//measurments
Matrix <double>[] x_and_P = filter.Update(f, x, P, h, z, Q, R); //ukf
x = x_and_P[0];
P = x_and_P[1];
Measurements.Add(new Measurement()
{
Value = z[0, 0], Time = TimeSpan.FromSeconds(k)
});
Estimates.Add(new Measurement()
{
Value = x_and_P[0][0, 0], Time = TimeSpan.FromSeconds(k), Variance = x_and_P[1][0, 0]
});
}
}
static void Main(string[] args)
{
var filter = new UKF();
List <double> measurements = new List <double>();
List <double> states = new List <double>();
Random rnd = new Random();
for (int k = 0; k < 100; k++)
{
var measurement = Math.Sin(k * 3.14 * 5 / 180) + (double)rnd.Next(50) / 100;
measurements.Add(measurement);
filter.Update(new[] { measurement });
states.Add(filter.getState()[0]);
}
GraphPane myPane = new GraphPane(new RectangleF(0, 0, 3200, 2400), "Unscented Kalman Filter", "number", "measurement");
PointPairList measurementsPairs = new PointPairList();
PointPairList statesPairs = new PointPairList();
for (int i = 0; i < measurements.Count; i++)
{
measurementsPairs.Add(i, measurements[i]);
statesPairs.Add(i, states[i]);
}
myPane.AddCurve("measurement", measurementsPairs, Color.Red, SymbolType.Circle);
myPane.AddCurve("estimate", statesPairs, Color.Green, SymbolType.XCross);
Bitmap bm = new Bitmap(200, 200);
Graphics g = Graphics.FromImage(bm);
myPane.AxisChange(g);
Image im = myPane.Image;
im.Save("result.png", ImageFormat.Png);
}
