Exemplo n.º 1
0
        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);
            }
        }