卡尔曼滤波（Kalman Filter）C#测试

一、操作过程       

刚学了一下卡尔曼滤波，具体原理还没细看，大致过程如下

分为两步，第一步Predict，以下两个公式

第二步Correct，以下三个公式

公式看起来很复杂，其中是我们要处理的数据， 是滤波之后的值，其他一些有些是需要给定的，有些是中间值。

        从t=1时刻开始，通过第一步，计算得到的值，给到第二步Correct里，第二步的第二个公式得到t=1时刻的 ，即为滤波后的值，t=2时刻，再把第二步在t=1时刻的Correct的那些值，代入到第一步Predict里在计算，再Correct，以此往复，得到所有t时刻的值。

二、代码实现

        试着用C#编写代码实现了下，实际值用了50个数，大致分布是x的平方加一个（0-100）的随机数，如下图黄色线条。 预测值为蓝色线条。

   

void testKF()
{
    double[] xhat = new double[50];   //x 滤波估计值
    double[] P = new double[50];      //滤波估计值协方差矩阵
    double[] xhatminus = new double[50];   //x 估计值
    double[] Pminus = new double[50];    //估计协方差矩阵
    double[] K = new double[50];    //卡尔曼增益
 
    double R = 0.1;  //测量噪音协方差 R一般可以观测得到，是滤波器的已知条件
 
    double Q = 0.01;  //过程激励噪声协方差（系统过程的协方差）。
                         //该参数被用来表示状态转换矩阵与实际过程之间的误差。
                         //因为我们无法直接观测到过程信号， 所以 Q 的取值是很难确定的。
                         //是卡尔曼滤波器用于估计离散时间过程的状态变量，也叫预测模型本身带来的噪声,状态转移协方差矩阵
 
 
    double[] x = new double[50];  //真实值加噪音
    Random r1 = new Random();           
    for (int i = 0; i < 50; i++)
    {
        x[i] = i*i + r1.NextDouble()*100;
    }
 
    P[0] = 1.0;
    xhat[0] = 0.0;
 
    double A=1,H=1;
 
    for(int i = 1;i < 50;i++)
    {
        //预测
        xhatminus[i] = A*xhat[i - 1];
        Pminus[i] = A*P[i - 1] + Q;
 
        //更新
        K[i] = Pminus[i]*H / (H*Pminus[i]*H + R);
        xhat[i] = xhatminus[i] + K[i] * (x[i] - H*xhatminus[i]);
        P[i] = (1 - K[i]*H) * Pminus[i];
    }
 
 
    chart1.Series[0].Points.Clear();
    chart1.Series[0].Name = "预测值";
    
 
    for (int i = 1; i < xhatminus.Length; i++)
    {
        chart1.Series[0].Points.AddXY(i, xhatminus[i]);
 
    }
 
    Series series = new Series();
    series.ChartType = SeriesChartType.Spline;
    series.Name = "实际值";
    for (int i = 1; i < xhatminus.Length; i++)
    {
        series.Points.AddXY(i, x[i]);
 
    }
 
    chart1.Series.Add(series);
 
}

OK，晚点再细看下原理