public static void NextStateEstimate(KalmanFilter kalman_filter, string time, Dictionary <string, DataContainer> estimates, Dictionary <string, DataContainer> received_packets)
{
// estimate states
double z = Convert.ToDouble(received_packets["yc1"].GetLastValue());
double u = Convert.ToDouble(received_packets["u1"].GetLastValue());
double[,] x = kalman_filter.Update(z, u);
// store the value
if (estimates.ContainsKey("yo1_hat"))
{
estimates["yo1_hat"].InsertData(time, x[0, 0].ToString());
}
if (estimates.ContainsKey("yc1_hat"))
{
estimates["yc1_hat"].InsertData(time, x[1, 0].ToString());
}
// store the residual
if (received_packets.ContainsKey("yc1"))
{
received_packets["yc1"].InsertResidual(kalman_filter.innovation.ToString());
received_packets["yc1"].InsertSecurityMetric(kalman_filter.security_metric.ToString());
}
}
public void ParseReceivedMessage(string message)
{
string time = "";
string text = message;
// split the message with the delimiter '#'
string[] container = text.Split('#');
foreach (string item in container)
{
// split each subtext (key and value)
string[] subitem = item.Split('_');
// extract key and value
string key = subitem[0];
string value = subitem[1];
// detect the time (don't add it as a separate key)
if (key == "time")
{
time = value; continue;
}
// if the key doesn't exist, add it
if (received_packets.ContainsKey(key) == false)
{
received_packets.Add(key, new DataContainer(Constants.n_datapoints));
// add an eventual residual and estimate continer
if (DEF_residual_states.Contains(key) == true)
{
received_packets[key].has_residual = true;
Helpers.AddEstimatesKey(key, estimates, Constants.n_datapoints);
if (key == "yc1")
{
Helpers.AddEstimatesKey("yo1", estimates, Constants.n_datapoints); // also estimate yo1 (hardcoded)
}
}
// increment the controlled states counter if it's a controlled state
if (DEF_controlled_states.Contains(key))
{
n_controlled_states += 1;
Helpers.AddReferencesKey(n_controlled_states, references, Constants.n_datapoints);
}
// flag that the GUI is receiving packets originating from the process
if (DEF_plant_states.Contains(key))
{
is_connected_to_plant = true;
}
}
// store the value
received_packets[key].InsertData(time, value);
}
// add reference time-stamp
if (references.ContainsKey("r1"))
{
references["r1"].CopyAndPushArray();
}
if (references.ContainsKey("r2"))
{
references["r2"].CopyAndPushArray();
}
// update state estimator and calculate residual
if (Main.plant_visualization == PlantVisualization.DOUBLE_WATERTANK)
{
KalmanFilter.NextStateEstimate(kalman_filter, time, estimates, received_packets);
}
}