卡尔曼滤波算法

           

 List<decimal?> waters = new List<decimal?>();

            foreach (sw_flowdata_self item in selfList)
            {
                waters.Add(item.waterLevel);
            }
            SynHelper syn = new SynHelper();

            double[] data = syn.GetKalMan(waters.ToArray());

    #region 卡尔曼滤波
        /// <summary>
        /// 卡尔曼滤波
        /// </summary>
        /// <param name="values"></param>
        /// <returns></returns>
        public double[] GetKalMan(decimal?[] values)
        {
            double[] CanShu ={ 23, 9, 16, 16, 1, 0, 0, 0 };
            double KamanX = CanShu[0];
            double KamanP = CanShu[1];
            double KamanQ = CanShu[2];
            double KamanR = CanShu[3];
            double KamanY = CanShu[4];
            double KamanKg = CanShu[5];
            double KamanSum = CanShu[6];
            double[] data = new double[values.Length];
            for (int i = 0; i <= values.Length - 1; i++)
            {
                KamanY = KamanX;
                KamanP = KamanP + KamanQ;
                KamanKg = KamanP / (KamanP + KamanR);
                KamanX = (KamanY + KamanKg * (Convert.ToDouble(values[i]) - KamanY));
                KamanSum += KamanX;
                data[i] = KamanX;
                KamanP = (1 - KamanKg) * KamanP;
            }
            return data;
        }
        #endregion