Unity 3D :卡爾曼濾波 ( Kalman Filter )

前言 :

卡爾曼濾波，沒啥好介紹的，網路上應該有很多人做介紹，如果你不知道卡爾曼濾波是啥，可以先去看別人寫的介紹，嘛…知道的大概就行。

但你可能會遇到 : 網路上的介紹，真正能用的程式碼太少，都是他媽的數學推導，又他媽的老闆又壓 Dead Line…，說要在指定時間內完成…Orz。

所以我做一個 C# Library 給大家使用，簡單易用。以貢獻世界，造福人群。 ( 灑花 )

回歸正題 : KalmanFilter_Test.cs 是執行腳本 ( 你的程序邏輯 ) ，KalmanFilter.cs 則是 Library。至於使用方法可以直接看 KalmanFilter_Test.cs，很簡單的淺而以懂，比我在上面這講廢話來的快。

執行結果 :

KalmanFilter_Test.cs :

// 本程式碼由黃彥霖所有，轉載請註明出處 : https://blog.csdn.net/weixin_38884324
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class KalmanFilter_Test : MonoBehaviour
{
    void Start()
    {
        float[] data = new float[] { 5, 6, 8, 3, 7, 6, 2 }; // 先給一組數字, 當然之後你也可以把 Sensor 讀到的值放入這

        KalmanFilter kf = new KalmanFilter();

        kf.SetQ(0.00001f); // 理想誤差 : 數值越大, 濾波效果越小
        kf.SetR(0.0001f);  // 實際誤差

        float[] s = kf.Filter(data); // 陣列濾波

        kf.SetFirst(data[0]); // [ 可選 ] 是否要先加入 "前一個" 值

        string ss = "實際數值\t陣列濾波\t即時濾波\n\n";

        for (int i = 1; i < data.Length; i++)
        {
            float d = kf.Filter(data[i]); // 即時濾波
            ss += data[i] + "\t" + s[i] + "\t" + d + "\n";
        }

        print(ss);
    }
}

KalmanFilter.cs :

// 本程式碼由黃彥霖所有，轉載請註明出處 : https://blog.csdn.net/weixin_38884324
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class KalmanFilter
{
    public float[] Filter(float[] z)
    {
        float[] xhat = new float[z.Length];
        xhat[0] = z[0];
        float P = 1;

        for (int k = 1; k < xhat.Length; k++)
        {
            float xhatminus = xhat[k - 1];
            float Pminus = P + Q;
            float K = Pminus / (Pminus + R);
            xhat[k] = xhatminus + K * (z[k] - xhatminus);
            P = (1 - K) * Pminus;
        }
        return xhat;
    }

    bool isFirst = true;
    bool haveSetFirst = false;
    float xhat;
    float P;
    float z0;
    float Q = 1e-5f; // 0.00001
    float R = 0.0001f;

    public void SetFirst(float z0)
    {
        this.z0 = z0;
        haveSetFirst = true;
    }

    // 理想誤差
    public void SetQ(float Q) {
        this.Q = Q;
    }

    // 實際誤差
    public void SetR(float R) {
        this.R = R;
    }

    public float Filter(float z1)
    {
        if (isFirst)
        {
            isFirst = false;
            if (haveSetFirst == false) z0 = z1;
            xhat = z0;
            P = 1;
        }
        
        float xhatminus = xhat;
        float Pminus = P + Q;
        float K = Pminus / (Pminus + R);
        xhat = xhatminus + K * (z1 - xhatminus);
        P = (1 - K) * Pminus;

        return xhat;
    }

    public void Reset()
    {
        isFirst = true;
        haveSetFirst = false;
        xhat = 0;
        P = 0;
        z0 = 0;
        Q = 1e-5f; // 0.00001
        R = 0.0001f;
    }
}