public void TestDiscreteKalmanFilter()
{
// Test constants
double r = 30.0; // Measurement covariance
double T = 20.0; // Time interval between measurements
double q = 0.1; // Plant noise constant
double tol = 0.0001; // Accuracy tolerance
// Reference values to test against (generated from a known filter)
// Reference Measurements
double[] zs = { 290.16851039,654.55633793,968.97141280,1325.09197161,1636.35947675,1974.39053148,2260.80770553,2574.36119750,2901.32285462,3259.14709098};
// Expected position estimates (predictions)
double[] posp = {1018.94416547,1237.00029618,1754.97092716,1855.62596430,2400.27521403,2446.47067625,2978.94381631,3173.63724675};
// Expected velocity estimates (predictions)
double[] velp = { 18.21939138,13.38351136,21.52280841,10.92729947,21.32868461,9.24370334,20.26482836,13.59419761 };
// Expected position estimates (after measurement update)
double[] posu = { 969.33006892,1324.51475894,1637.07997492,1973.70152187,2261.59660945,2573.64724909,2901.75329465,3258.67447647 };
// Expected velocity estimates (after measurement update)
double[] velu = { 13.38351136,21.52280841,10.92729947,21.32868461,9.24370334,20.26482836,13.59419761,20.93270702 };
// Initial estimate based on two point differencing
double z0 = zs[0];
double z1 = zs[1];
Matrix<double> x0 = Matrix<double>.Build.Dense(2, 1, new[] { z1, (z1 - z0)/T });
Matrix<double> P0 = Matrix<double>.Build.Dense(2, 2, new[] { r, r/T, r/T, 2*r/(T*T) });
// Setup a DiscreteKalmanFilter to filter
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
Matrix<double> F = Matrix<double>.Build.Dense(2, 2, new[] { 1d, 0d, T, 1 }); // State transition matrix
Matrix<double> G = Matrix<double>.Build.Dense(2, 1, new[] { (T * T) / 2d, T }); // Plant noise matrix
Matrix<double> Q = Matrix<double>.Build.Dense(1, 1, new[] { q }); // Plant noise variance
Matrix<double> R = Matrix<double>.Build.Dense(1, 1, new[] { r }); // Measurement variance matrix
Matrix<double> H = Matrix<double>.Build.Dense(1, 2, new[] { 1d, 0d }); // Measurement matrix
// Test the performance of this filter against the stored data from a proven
// Kalman Filter of a one dimenional tracker.
for (int i = 2; i < zs.Length; i++)
{
// Perform the prediction
dkf.Predict(F, G, Q);
// Test against the prediction state/covariance
Assert.IsTrue(dkf.State[0,0].AlmostEqual(posp[i-2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(dkf.State[1, 0].AlmostEqual(velp[i-2], tol), "Covariance Prediction (" + i + ")");
// Perform the update
Matrix<double> z = Matrix<double>.Build.Dense(1, 1, new[] { zs[i] });
dkf.Update(z, H, R);
// Test against the update state/covariance
// Test against the prediction state/covariance
Assert.IsTrue(dkf.State[0, 0].AlmostEqual(posu[i-2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(dkf.State[1, 0].AlmostEqual(velu[i-2], tol), "Covariance Prediction (" + i + ")");
}
}
public LandmarkFilter(double timeInterval, double noise)
{
ModelState initialState = new ModelState
{
Position = new Vector3(0, 0, 0),
Velocity = new Vector3(0, 0, 0),
//Acceleration = new Vector3(0, 0, 0),
};
double[,] initialStateError = ModelState.GetProcessNoise(noise, timeInterval);
int measurementVectorDimension = 3;
int controlVectorDimension = 0;
Func <ModelState, double[]> stateConvertFunc = ModelState.ToArray;
Func <double[], ModelState> stateConvertBackFunc = ModelState.FromArray;
Func <NormalizedLandmark, double[]> measurementConvertFunc = v => { return(new double[3] {
v.X, v.Y, v.Z
}); };
kalmanFilter = new DiscreteKalmanFilter <ModelState, NormalizedLandmark>(
initialState,
initialStateError,
measurementVectorDimension,
controlVectorDimension,
stateConvertFunc,
stateConvertBackFunc,
measurementConvertFunc);
kalmanFilter.ProcessNoise = ModelState.GetProcessNoise(noise, timeInterval);
kalmanFilter.MeasurementNoise = Matrix.Diagonal <double>(kalmanFilter.MeasurementVectorDimension, 1);
kalmanFilter.MeasurementMatrix = ModelState.GetPositionMeasurementMatrix();
kalmanFilter.TransitionMatrix = ModelState.GetTransitionMatrix(timeInterval);
kalmanFilter.Predict();
}
public void TestDiscreteKalmanFilter()
{
// Test constants
double r = 30.0; // Measurement covariance
double T = 20.0; // Time interval between measurements
double q = 0.1; // Plant noise constant
double tol = 0.0001; // Accuracy tolerance
// Reference values to test against (generated from a known filter)
// Reference Measurements
double[] zs = { 290.16851039, 654.55633793, 968.97141280, 1325.09197161, 1636.35947675, 1974.39053148, 2260.80770553, 2574.36119750, 2901.32285462, 3259.14709098 };
// Expected position estimates (predictions)
double[] posp = { 1018.94416547, 1237.00029618, 1754.97092716, 1855.62596430, 2400.27521403, 2446.47067625, 2978.94381631, 3173.63724675 };
// Expected velocity estimates (predictions)
double[] velp = { 18.21939138, 13.38351136, 21.52280841, 10.92729947, 21.32868461, 9.24370334, 20.26482836, 13.59419761 };
// Expected position estimates (after measurement update)
double[] posu = { 969.33006892, 1324.51475894, 1637.07997492, 1973.70152187, 2261.59660945, 2573.64724909, 2901.75329465, 3258.67447647 };
// Expected velocity estimates (after measurement update)
double[] velu = { 13.38351136, 21.52280841, 10.92729947, 21.32868461, 9.24370334, 20.26482836, 13.59419761, 20.93270702 };
// Initial estimate based on two point differencing
double z0 = zs[0];
double z1 = zs[1];
Matrix x0 = new Matrix(2, 1);
x0[0, 0] = z1;
x0[1, 0] = (z1 - z0) / T;
Matrix P0 = new Matrix(2, 2);
P0[0, 0] = r; P0[1, 0] = r / T; P0[0, 1] = r / T; P0[1, 1] = 2 * r / (T * T);
// Setup a DiscreteKalmanFilter to filter
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
double[] aF = { 1d, 0d, T, 1 };
double[] aG = { (T * T) / 2d, T };
Matrix F = new Matrix(aF, 2); // State transition matrix
Matrix G = new Matrix(aG, 2); // Plant noise matrix
Matrix Q = new Matrix(1, 1); Q[0, 0] = q; // Plant noise variance
Matrix R = new Matrix(1, 1); R[0, 0] = r; // Measurement variance matrix
Matrix H = new Matrix(1, 2); H[0, 0] = 1d; H[0, 1] = 0d; // Measurement matrix
// Test the performance of this filter against the stored data from a proven
// Kalman Filter of a one dimenional tracker.
for (int i = 2; i < zs.Length; i++)
{
// Perform the prediction
dkf.Predict(F, G, Q);
// Test against the prediction state/covariance
Assert.IsTrue(Number.AlmostEqual(dkf.State[0, 0], posp[i - 2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(Number.AlmostEqual(dkf.State[1, 0], velp[i - 2], tol), "Covariance Prediction (" + i + ")");
// Perform the update
Matrix z = new Matrix(1, 1, zs[i]);
dkf.Update(z, H, R);
// Test against the update state/covariance
// Test against the prediction state/covariance
Assert.IsTrue(Number.AlmostEqual(dkf.State[0, 0], posu[i - 2], tol), "State Prediction (" + i + ")");
Assert.IsTrue(Number.AlmostEqual(dkf.State[1, 0], velu[i - 2], tol), "Covariance Prediction (" + i + ")");
}
}
public TransformFilter(Vector3 initialPosition, Quaternion initialRotation, int framerate, float noise = 10f, float reliability = 1.3f)
{
this.framerate = framerate;
positionFilter = KalmanGenerator.Generate3DimensionalFilter(initialPosition, framerate, noise, reliability);
forwardRotationFilter = KalmanGenerator.Generate3DimensionalFilter(initialRotation * Vector3.forward, framerate, noise, reliability);
upRotationFilter = KalmanGenerator.Generate3DimensionalFilter(initialRotation * Vector3.up, framerate, noise, reliability);
}
public static Tuple <Matrix <double>, Matrix <double> > PredictState(this DiscreteKalmanFilter dFilterSharp, TimeSpan ts, Matrix <double> F, Matrix <double> G, Matrix <double> Q)
{
// F.UpdateTransition(ts);
dFilterSharp.Predict(F, G, Q);
return(Tuple.Create(dFilterSharp.State, dFilterSharp.Cov));
}
public static void UpdateQ(this DiscreteKalmanFilter dFilterSharp, IAdaptiveQ adaptiveQ, TimeSpan ts, Matrix <double> z, Matrix <double> H, Matrix <double> R)
{
//Matrix<double> z = mBuilder.DenseOfColumnArrays(newMeas);
var innovation = z - H * dFilterSharp.State;
var kalmanGain = dFilterSharp.Cov * H.Transpose() * (H * dFilterSharp.Cov * H.Transpose() + R).Inverse();
adaptiveQ.Update(ts, innovation, kalmanGain);/*Map(_ => Math.Abs(_)).AsColumnMajorArray();*/
}
public void TestSquareRootFilter()
{
System.Console.WriteLine("Filter 1 - DiscreteKalmanFilter, Filter 2 - SquareRootFilter");
Matrix x0 = RangeBearingTracker.TwoPointDifferenceState(rM[0], rM[1], bM[0], bM[1], T);
Matrix P0 = RangeBearingTracker.TwoPointDifferenceCovariance(rM[0], rM[1], bM[0], bM[1], re, the, T);
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
SquareRootFilter sqrf = new SquareRootFilter(x0, P0);
Assert.IsTrue(RunTest(dkf, sqrf, DefaultTolerance));
}
public void TestInformationFilter()
{
System.Console.WriteLine("Filter 1 - DiscreteKalmanFilter, Filter 2 - InformationFilter");
Matrix <double> x0 = RangeBearingTracker.TwoPointDifferenceState(rM[0], rM[1], bM[0], bM[1], T);
Matrix <double> P0 = RangeBearingTracker.TwoPointDifferenceCovariance(rM[0], rM[1], bM[0], bM[1], re, the, T);
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
InformationFilter inf = new InformationFilter(x0, P0);
Assert.IsTrue(RunTest(dkf, inf, DefaultTolerance));
}
public Mapping(Context cntxt)
{
context = cntxt;
activity = (MainActivity)cntxt;
velocity = 0;
velocityXYZ = new float[] { 0, 0, 0 };
positionXYZ = new float[] { 0, 0, 0 };
for (int i = 0; i < 3; i++)
{
accelHist.Add(new List <float>());//Add a XYZ history
}
sw = new Stopwatch();
stateOld = state = getStateMat(velocityXYZ);
stateCov = getStateCov();
KFilter = new DiscreteKalmanFilter(state, stateCov);
}
private void initialize(double processNoise, double measNoise, DateTime dt, double initialPosition)
{
accelNoise = processNoise;
measurementNoise = measNoise;
State = new ConstantVelocity1DModel(dt.Ticks, initialPosition);/*process.GetNoisyState(accelNoise); //assuming we measured process params (noise)*/
var initialStateError = State.GetProcessNoise(accelNoise);
kalman = new DiscreteKalmanFilter <ModelState, double>(State, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => x.ToArray(), x => ModelState.FromArray(x), x => new double[] { x });
kalman.ProcessNoise = State.GetProcessNoise(accelNoise);
kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, measurementNoise).Power(2); //assuming we measured process params (noise) - ^2 => variance
kalman.MeasurementMatrix = new double[, ] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
{ 1, 0 }
};
}
public static Tuple <Matrix <double>, Matrix <double> > UpdateState(this DiscreteKalmanFilter dFilterSharp, Matrix <double> z, Matrix <double> H, Matrix <double> R)
{
dFilterSharp.Update(z, H, R);
return(Tuple.Create(dFilterSharp.State, dFilterSharp.Cov));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initialLocation">Initial location object</param>
public DiscreteKalman2d(Location2d initialLocation)
{
_location = initialLocation ?? throw new ArgumentException("Initial location required");
dkf = new DiscreteKalmanFilter(initialLocation.Initial, initialLocation.Cov);
}
public void TestSquareRootFilter()
{
System.Console.WriteLine("Filter 1 - DiscreteKalmanFilter, Filter 2 - SquareRootFilter");
Matrix<double> x0 = RangeBearingTracker.TwoPointDifferenceState(rM[0], rM[1], bM[0], bM[1], T);
Matrix<double> P0 = RangeBearingTracker.TwoPointDifferenceCovariance(rM[0], rM[1], bM[0], bM[1], re, the, T);
DiscreteKalmanFilter dkf = new DiscreteKalmanFilter(x0, P0);
SquareRootFilter sqrf = new SquareRootFilter(x0, P0);
Assert.IsTrue(RunTest(dkf, sqrf, DefaultTolerance));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="initial_x">Initial x location</param>
/// <param name="initial_y">Initial y location</param>
/// <param name="covariance">Initial covariance value</param>
public DiscreteKalman2d(double initial_x, double initial_y, double covariance)
{
_location = new Location2d(initial_x, initial_y);
_location.SetCovarianceDiagonal(covariance);
dkf = new DiscreteKalmanFilter(_location.Initial, _location.Cov);
}
static void Main(string[] args)
{
double[] IndividualValues = { 0, 0, 0, 0, 0, 0 };
String[] SubStrings;
float[] q = { 0, 0, 0, 0 };
double x = 0, xq, y = 0, z = 0, t2 = 0, x1, y1, z1, time, velx, vely, velz, accx, accy, accz, temp;
bool correctRead;
int comaCount = 0;
char[] test;
SerialPort port = new SerialPort("COM3", 19200);
String CurrentReading = "ABCD";
Matrix initial = Matrix.Build.DenseOfArray(new double[, ] {
{ 0 },
{ 0 },
{ 0 },
{ 0 },
{ 0 },
{ 0 },
{ 0 },
{ 0 },
{ 0 }
});
Matrix covariance = Matrix.Build.DenseOfArray(new double[, ] {
{ 0.1, 0, 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0.1, 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0.1, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0.1, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0.1, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0.1, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0.1, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0.1, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0.1 }
});
DiscreteKalmanFilter kalman = new DiscreteKalmanFilter(initial, covariance);
port.ReadTimeout = 2000;
port.Close();
if (!port.IsOpen)
{
port.Open();
}
while (t2 <= 20000000000)
{
CurrentReading = port.ReadLine();
test = CurrentReading.ToCharArray();
correctRead = true;
comaCount = 0;
for (int i = test.Length - 1; i >= 0; i--)
{
if (Char.IsNumber(test[i]) == false & test[i] != ',' & test[i] != '.' & Char.IsWhiteSpace(test[i]) == false & test[i] != '-')
{
correctRead = false;
}
if (test[i] == ',')
{
comaCount = comaCount + 1;
}
}
if (correctRead == true & comaCount == 9)
{
SubStrings = CurrentReading.Split(',');
q[0] = Convert.ToSingle(SubStrings[0]);
q[1] = Convert.ToSingle(SubStrings[1]);
q[2] = Convert.ToSingle(SubStrings[2]);
q[3] = Convert.ToSingle(SubStrings[3]);
Quaternion quaternion = new Quaternion(q[0], q[1], q[2], q[3]);
x1 = Convert.ToDouble(SubStrings[4]);
y1 = Convert.ToDouble(SubStrings[5]);
z1 = Convert.ToDouble(SubStrings[6]);
temp = Convert.ToDouble(SubStrings[7]);
time = Convert.ToSingle(SubStrings[8]);
velx = (x1 - x) / (time - t2);
vely = (y1 - y) / (time - t2);
velz = (z1 - z) / (time - t2);
accx = velx / (time - t2);
accy = vely / (time - t2);
accz = velz / (time - t2);
//Dummy Data : Generate a very noisy position estimate
Matrix Meassurement = Matrix.Build.DenseOfArray(new double[, ] {
{ 0.1 },
{ 0.1 },
{ 0.1 },
{ 0.1 },
{ 0.1 },
{ 0.1 },
{ 0.1 },
{ 0.1 },
{ 0.1 }
});
Matrix Noise = Matrix.Build.DenseOfArray(new double[, ] {
{ 50.94324709, 0, 0, 0, 0, 0, 0, 0, 0 },
{ 0, 345.3342043, 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 184.5258328, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0.000105439, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0.000107133, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0.000586316, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 5.7569E-06, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 1.20007E-05, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 3.71021E-052 }
});
//veryfie
Matrix startetrans = Matrix.Build.DenseOfArray(new double[, ] {
{ x1, 0, 0, 0, 0, 0, 0, 0, 0 },
{ 0, y1, 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, z1, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, velx, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, vely, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, velz, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, accx, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, accy, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, accz }
});
kalman.Predict(startetrans, kalman.Cov);
kalman.Update(Meassurement, Noise, covariance);
x1 += kalman.State.At(0, 0);
y1 += kalman.State.At(1, 0);
z1 += kalman.State.At(2, 0);
Console.WriteLine("x: " + quaternion.ToEulerAngles().Alpha.Degrees + ", y: " + quaternion.ToEulerAngles().Beta.Degrees + ", z: " + z1 + ", temperature: " + temp + ", time: " + t2);
t2 = time;
x = x1;
y = y1;
z = z1;
}
}
}
public RealtimeEngine()
{
nsamples = new int[MainClass.devices.Length];
bool usehpf = MainClass.win._handles.useHPF.Active;
bool uselpf = MainClass.win._handles.useLPF.Active;
double hpf = 0.016;
try
{
hpf = Convert.ToDouble(MainClass.win._handles.editHPF.Text);
}
catch
{
usehpf = false;
}
double lpf = 0.5;
try
{
lpf = Convert.ToDouble(MainClass.win._handles.editLPF.Text);
}
catch
{
uselpf = false;
}
fs = new double[MainClass.devices.Length];
OnlineFIRFiltersHPF = new OnlineFilter[MainClass.devices.Length][];
OnlineFIRFiltersHPF2 = new OnlineFilter[MainClass.devices.Length][];
OnlineFIRFiltersLPF = new OnlineFilter[MainClass.devices.Length][];
MocoKalman = new DiscreteKalmanFilter[MainClass.devices.Length][];
mBLLmappings = new MBLLmapping[MainClass.devices.Length];
for (int i = 0; i < MainClass.devices.Length; i++)
{
int nSDpairs = MainClass.win.nirsdata[i].probe.numChannels / MainClass.win.nirsdata[i].probe.numWavelengths;
mBLLmappings[i] = new MBLLmapping();
mBLLmappings[i].distances = new double[nSDpairs];
mBLLmappings[i].hboIndex = new int[nSDpairs][];
mBLLmappings[i].measurementPairs = new int[nSDpairs][];
mBLLmappings[i].ExtCoefHbO = new double[nSDpairs][];
mBLLmappings[i].ExtCoefHbR = new double[nSDpairs][];
int cnt = 0;
int[] cnt2 = new int[nSDpairs];
for (int ii = 0; ii < nSDpairs; ii++)
{
cnt2[ii] = 0;
mBLLmappings[i].hboIndex[ii] = new int[2];
mBLLmappings[i].measurementPairs[ii] = new int[MainClass.win.nirsdata[i].probe.numWavelengths];
mBLLmappings[i].ExtCoefHbO[ii] = new double[MainClass.win.nirsdata[i].probe.numWavelengths];
mBLLmappings[i].ExtCoefHbR[ii] = new double[MainClass.win.nirsdata[i].probe.numWavelengths];
}
nirs.Media media = new nirs.Media();
for (int sI = 0; sI < MainClass.win.nirsdata[i].probe.numSrc; sI++)
{
for (int dI = 0; dI < MainClass.win.nirsdata[i].probe.numDet; dI++)
{
bool found = false;;
for (int mI = 0; mI < MainClass.win.nirsdata[i].probe.numChannels; mI++)
{
if (MainClass.win.nirsdata[i].probe.ChannelMap[mI].sourceindex == sI &
MainClass.win.nirsdata[i].probe.ChannelMap[mI].detectorindex == dI)
{
mBLLmappings[i].measurementPairs[cnt][cnt2[cnt]] = mI + MainClass.win.nirsdata[i].probe.numChannels;
if (mBLLmappings[i].hboIndex[cnt][0] == 0)
{
mBLLmappings[i].hboIndex[cnt][0] = mI + MainClass.win.nirsdata[i].probe.numChannels * 2;
mBLLmappings[i].hboIndex[cnt][1] = mI + nSDpairs + MainClass.win.nirsdata[i].probe.numChannels * 2;
}
mBLLmappings[i].distances[cnt] = 0;
media.GetSpectrum(MainClass.win.nirsdata[i].probe.ChannelMap[mI].wavelength,
out mBLLmappings[i].ExtCoefHbO[cnt][cnt2[cnt]], out mBLLmappings[i].ExtCoefHbR[cnt][cnt2[cnt]]);
mBLLmappings[i].distances[cnt] = Math.Sqrt((MainClass.win.nirsdata[i].probe.SrcPos[sI, 0] -
MainClass.win.nirsdata[i].probe.DetPos[dI, 0]) *
(MainClass.win.nirsdata[i].probe.SrcPos[sI, 0] -
MainClass.win.nirsdata[i].probe.DetPos[dI, 0]) +
(MainClass.win.nirsdata[i].probe.SrcPos[sI, 1] -
MainClass.win.nirsdata[i].probe.DetPos[dI, 1]) *
(MainClass.win.nirsdata[i].probe.SrcPos[sI, 1] -
MainClass.win.nirsdata[i].probe.DetPos[dI, 1]));
cnt2[cnt]++;
found = true;
}
}
if (found)
{
cnt++;
}
}
}
nsamples[i] = 0;
NIRSDAQ.info info = MainClass.devices[i].GetInfo();
fs[i] = info.sample_rate;
OnlineFIRFiltersHPF[i] = new OnlineFilter[MainClass.win.nirsdata[i].probe.numChannels];
OnlineFIRFiltersLPF[i] = new OnlineFilter[MainClass.win.nirsdata[i].probe.numChannels];
OnlineFIRFiltersHPF2[i] = new OnlineFilter[MainClass.win.nirsdata[i].probe.numChannels];
MocoKalman[i] = new DiscreteKalmanFilter[MainClass.win.nirsdata[i].probe.numChannels];
for (int j = 0; j < MainClass.win.nirsdata[i].probe.numChannels; j++)
{
OnlineFIRFiltersLPF[i][j] = OnlineFilter.CreateLowpass(ImpulseResponse.Finite, fs[i], lpf);
OnlineFIRFiltersHPF[i][j] = OnlineFilter.CreateHighpass(ImpulseResponse.Finite, fs[i], hpf);
OnlineFIRFiltersHPF2[i][j] = OnlineFilter.CreateHighpass(ImpulseResponse.Finite, fs[i], 0.001);
OnlineFIRFiltersHPF[i][j].Reset();
OnlineFIRFiltersHPF2[i][j].Reset();
OnlineFIRFiltersLPF[i][j].Reset();
int order = 5;
var x0 = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.Dense(order + 1, 1, 0);
var P0 = MathNet.Numerics.LinearAlgebra.Matrix <double> .Build.DenseIdentity(order + 1);
MocoKalman[i][j] = new DiscreteKalmanFilter(x0, P0);
}
}
}
public static void UpdateR(this DiscreteKalmanFilter dFilterSharp, IAdaptiveR adaptiveR, TimeSpan ts, Matrix <double> z, Matrix <double> H)
{
var residual = z - H * dFilterSharp.State;
adaptiveR.Update(ts, residual, H, dFilterSharp.Cov);/*Map(_ => Math.Abs(_)).AsColumnMajorArray();*/
}
