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); } }