NO.48------ C#实现卡尔曼滤波

最新推荐文章于 2024-11-17 11:08:24 发布

one named slash

最新推荐文章于 2024-11-17 11:08:24 发布

阅读量4.6k

点赞数 4

CC 4.0 BY-SA版权

分类专栏： Unity3D 文章标签：卡尔曼滤波 Unity

本文链接：https://blog.youkuaiyun.com/ghl1390490928/article/details/89283084

Unity3D 专栏收录该内容

7 篇文章

订阅专栏

本文介绍在Unity项目中应用卡尔曼滤波技术消除头部跟踪时的抖动问题，通过处理Dlib实时获取的68个特征点，实现更平滑的头部运动追踪效果。

通俗的讲，卡尔曼滤波就是综合上一步骤对当前步骤的预测值，结合当前步骤的测量值，来估计当前的状态。

最近在做一个Unity项目时，用到了卡尔曼滤波消除抖动,主要是消除头部抖动，从Dlib实时获得68个特征值的points，由points.x points.y points.z 获得头部的旋转四元数作为测量值，初始值可以任意定义，因为几次迭代后预测值就逼近真实值了。因为Unity用c#编写，c#本身不存在矩阵库，对矩阵运算比较吃力，需要自己导入矩阵库，方法很简单，首先定义矩阵库的命名空间，然后使用该命名空间即可。

矩阵库：

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;


namespace Slash.Lastwork
{
    public class Matrix
    {
        double[,] A;
        //m行n列
        int m, n;
        string name;
        public Matrix(int am, int an)
        {
            m = am;
            n = an;
            A = new double[m, n];
            name = "Result";
        }
        public Matrix(int am, int an, string aName)
        {
            m = am;
            n = an;
            A = new double[m, n];
            name = aName;
        }

        public int getM
        {
            get { return m; }
        }
        public int getN
        {
            get { return n; }
        }
        public double[,] Detail
        {
            get { return A; }
            set { A = value; }
        }
        public string Name
        {
            get { return name; }
            set { name = value; }
        }
    }
    public class MatrixOperator
    {
        MatrixOperator()
        { }
        /// <summary>
        /// 矩阵加法
        /// </summary>
        /// <param name="Ma"></param>
        /// <param name="Mb"></param>
        /// <returns></returns>
        public static Matrix MatrixAdd(Matrix Ma, Matrix Mb)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            int m2 = Mb.getM;
            int n2 = Mb.getN;

            if ((m != m2) || (n != n2))
            {
                Exception myException = new Exception("数组维数不匹配");
                throw myException;
            }

            Matrix Mc = new Matrix(m, n);
            double[,] c = Mc.Detail;
            double[,] a = Ma.Detail;
            double[,] b = Mb.Detail;

            int i, j;
            for (i = 0; i < m; i++)
                for (j = 0; j < n; j++)
                    c[i, j] = a[i, j] + b[i, j];

            return Mc;
        }
        /// <summary>
        /// 矩阵减法
        /// </summary>
        /// <param name="Ma"></param>
        /// <param name="Mb"></param>
        /// <returns></returns>
        public static Matrix MatrixSub(Matrix Ma, Matrix Mb)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            int m2 = Mb.getM;
            int n2 = Mb.getN;

            if ((m != m2) || (n != n2))
            {
                Exception myException = new Exception("数组维数不匹配");
                throw myException;
            }

            Matrix Mc = new Matrix(m, n);
            double[,] c = Mc.Detail;
            double[,] a = Ma.Detail;
            double[,] b = Mb.Detail;

            int i, j;
            for (i = 0; i < m; i++)
                for (j = 0; j < n; j++)
                    c[i, j] = a[i, j] - b[i, j];

            return Mc;
        }
        /// <summary>
        /// 矩阵打印
        /// </summary>
        /// <param name="Ma"></param>
        /// <returns></returns>
        public static string MatrixPrint(Matrix Ma)
        {
            string s;
            s = Ma.Name + ":\n";

            int m = Ma.getM;
            int n = Ma.getN;

            double[,] a = Ma.Detail;

            for (int i = 0; i < m; i++)
            {
                for (int j = 0; j < n; j++)
                {
                    s += a[i, j].ToString("0.0000") + "\t";
                }
                s += "\n";
            }
            return s;
        }
        /// <summary>
        /// 矩阵乘法
        /// </summary>
        /// <param name="Ma"></param>
        /// <param name="Mb"></param>
        /// <returns></returns>
        public static Matrix MatrixMulti(Matrix Ma, Matrix Mb)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            int m2 = Mb.getM;
            int n2 = Mb.getN;

            if (n != m2)
            {
                Exception myException = new Exception("数组维数不匹配");
                throw myException;
            }

            Matrix Mc = new Matrix(m, n2);
            double[,] c = Mc.Detail;
            double[,] a = Ma.Detail;
            double[,] b = Mb.Detail;

            int i, j, k;
            for (i = 0; i < m; i++)
                for (j = 0; j < n2; j++)
                {
                    c[i, j] = 0;
                    for (k = 0; k < n; k++)
                        c[i, j] += a[i, k] * b[k, j];
                }

            return Mc;
        }
        /// <summary>
        /// 矩阵数乘
        /// </summary>
        /// <param name="k"></param>
        /// <param name="Ma"></param>
        /// <returns></returns>
        public static Matrix MatrixSimpleMulti(double k, Matrix Ma)
        {
            int n = Ma.getN;
            int m = Ma.getM;
            Matrix Mc = new Matrix(m, n);
            double[,] c = Mc.Detail;
            double[,] a = Ma.Detail;

            int i, j;
            for (i = 0; i < m; i++)
                for (j = 0; j < n; j++)
                    c[i, j] = a[i, j] * k;

            return Mc;
        }
        /// <summary>
        /// 矩阵转置
        /// </summary>
        /// <param name="Ma"></param>
        /// <param name="Mb"></param>
        /// <returns></returns>
        public static Matrix MatrixTrans(Matrix Ma)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            Matrix Mc = new Matrix(n, m);
            double[,] c = Mc.Detail;
            double[,] a = Ma.Detail;

            for (int i = 0; i < n; i++)
                for (int j = 0; j < m; j++)
                    c[i, j] = a[j, i];

            return Mc;
        }
        /// <summary>
        /// 矩阵求逆(高斯法)
        /// </summary>
        /// <param name="Ma"></param>
        /// <returns></returns>
        public static Matrix MatrixInv(Matrix Ma)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            if (m != n)
            {
                Exception myException = new Exception("数组维数不匹配");
                throw myException;
            }
            Matrix Mc = new Matrix(m, n);
            double[,] a0 = Ma.Detail;
            double[,] a = (double[,])a0.Clone();
            double[,] b = Mc.Detail;

            int i, j, row, k;
            double max, temp;

            //单位矩阵
            for (i = 0; i < n; i++)
            {
                b[i, i] = 1;
            }
            for (k = 0; k < n; k++)
            {
                max = 0; row = k;
                //找最大元，其所在行为row
                for (i = k; i < n; i++)
                {
                    temp = Math.Abs(a[i, k]);
                    if (max < temp)
                    {
                        max = temp;
                        row = i;
                    }

                }
                if (max == 0)
                {
                    Exception myException = new Exception("没有逆矩阵");
                    throw myException;
                }
                //交换k与row行
                if (row != k)
                {
                    for (j = 0; j < n; j++)
                    {
                        temp = a[row, j];
                        a[row, j] = a[k, j];
                        a[k, j] = temp;

                        temp = b[row, j];
                        b[row, j] = b[k, j];
                        b[k, j] = temp;
                    }

                }

                //首元化为1
                for (j = k + 1; j < n; j++) a[k, j] /= a[k, k];
                for (j = 0; j < n; j++) b[k, j] /= a[k, k];

                a[k, k] = 1;

                //k列化为0
                //对a
                for (j = k + 1; j < n; j++)
                {
                    for (i = 0; i < k; i++) a[i, j] -= a[i, k] * a[k, j];
                    for (i = k + 1; i < n; i++) a[i, j] -= a[i, k] * a[k, j];
                }
                //对b
                for (j = 0; j < n; j++)
                {
                    for (i = 0; i < k; i++) b[i, j] -= a[i, k] * b[k, j];
                    for (i = k + 1; i < n; i++) b[i, j] -= a[i, k] * b[k, j];
                }
                for (i = 0; i < n; i++) a[i, k] = 0;
                a[k, k] = 1;
            }

            return Mc;
        }
        /// <summary>
        /// 矩阵求逆(伴随矩阵法)
        /// </summary>
        /// <param name="Ma"></param>
        /// <returns></returns>
        public static Matrix MatrixInvByCom(Matrix Ma)
        {
            double d = MatrixOperator.MatrixDet(Ma);
            if (d == 0)
            {
                Exception myException = new Exception("没有逆矩阵");
                throw myException;
            }
            Matrix Ax = MatrixOperator.MatrixCom(Ma);
            Matrix An = MatrixOperator.MatrixSimpleMulti((1.0 / d), Ax);
            return An;
        }
        /// <summary>
        /// 对应行列式的代数余子式矩阵
        /// </summary>
        /// <param name="Ma"></param>
        /// <returns></returns>
        public static Matrix MatrixSpa(Matrix Ma, int ai, int aj)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            if (m != n)
            {
                Exception myException = new Exception("数组维数不匹配");
                throw myException;
            }
            int n2 = n - 1;
            Matrix Mc = new Matrix(n2, n2);
            double[,] a = Ma.Detail;
            double[,] b = Mc.Detail;

            //左上
            for (int i = 0; i < ai; i++)
                for (int j = 0; j < aj; j++)
                {
                    b[i, j] = a[i, j];
                }
            //右下
            for (int i = ai; i < n2; i++)
                for (int j = aj; j < n2; j++)
                {
                    b[i, j] = a[i + 1, j + 1];
                }
            //右上
            for (int i = 0; i < ai; i++)
                for (int j = aj; j < n2; j++)
                {
                    b[i, j] = a[i, j + 1];
                }
            //左下
            for (int i = ai; i < n2; i++)
                for (int j = 0; j < aj; j++)
                {
                    b[i, j] = a[i + 1, j];
                }
            //符号位
            if ((ai + aj) % 2 != 0)
            {
                for (int i = 0; i < n2; i++)
                    b[i, 0] = -b[i, 0];

            }
            return Mc;
        }
        /// <summary>
        /// 矩阵的行列式
        /// </summary>
        /// <param name="Ma"></param>
        /// <returns></returns>
        public static double MatrixDet(Matrix Ma)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            if (m != n)
            {
                Exception myException = new Exception("数组维数不匹配");
                throw myException;
            }
            double[,] a = Ma.Detail;
            if (n == 1) return a[0, 0];

            double D = 0;
            for (int i = 0; i < n; i++)
            {
                D += a[1, i] * MatrixDet(MatrixSpa(Ma, 1, i));
            }
            return D;
        }
        /// <summary>
        /// 矩阵的伴随矩阵
        /// </summary>
        /// <param name="Ma"></param>
        /// <returns></returns>
        public static Matrix MatrixCom(Matrix Ma)
        {
            int m = Ma.getM;
            int n = Ma.getN;
            Matrix Mc = new Matrix(m, n);
            double[,] c = Mc.Detail;
            double[,] a = Ma.Detail;

            for (int i = 0; i < m; i++)
                for (int j = 0; j < n; j++)
                    c[i, j] = MatrixDet(MatrixSpa(Ma, j, i));

            return Mc;
        }
    }


}

迭代过程：

using UnityEngine;
using System.Collections.Generic;
using System;
using System.IO;
using Slash.Lastwork;

#if UNITY_5_3 || UNITY_5_3_OR_NEWER
using UnityEngine.SceneManagement;
#endif
using OpenCVForUnity;
using DlibFaceLandmarkDetector;
using DlibFaceLandmarkDetectorSample;
using Moments;

namespace Slash.Facemoji
{
    /// <summary>
    /// FaceTracking, Use Dlib to detect face landmark and use Live2D model to track faces
    /// </summary>
    [AddComponentMenu("Facemoji/FaceTracking")]
    [RequireComponent (typeof(WebCamTextureToMatHelper))]
    public class FaceTracking : MonoBehaviour
    {
        ///矩阵的运算
        //F_K 状态转移矩阵
        Matrix stateConvert = new Matrix(6, 6, "stateConvert");
        //P_K 后验估计误差协方差矩阵，度量估计值的精确程度
        Matrix errorCovPost = new Matrix(6, 6, "errorCovPsot");
        //H_K 观测矩阵
        Matrix measureMatrix = new Matrix(6, 3, "measureMatrix");
        //Q_K 过程噪声
        Matrix processNoiseConvert = new Matrix(6, 6, "processNoiseConvert");
        //R_K 观测噪声
        Matrix measureNoiseConvert = new Matrix(3, 3, "measureNoiseConvert");
        //X_0 初始状态
        Matrix originState = new Matrix(3, 1, "originState");
        /// <summary>
        /// All Live2D Texture
        /// </summary>
        private enum WhichTexture
        {
            noir_santa = 1,
            uni = 2,
        };

        /// <summary>
        /// The selected Live2D Texture now.(Default shizuku)
        /// </summary>
        private WhichTexture selectedTexture = WhichTexture.uni;

        /// <summary>
        /// Start Button
        /// </summary>
        public static GameObject startBtn;

        /// <summary>
        /// Finish Button
        /// </summary>
        public static GameObject finishBtn;

        /// <summary>
        /// live2DModel.transform.localScale
        /// </summary>
        public float modelScale = 0.9f;

        /// <summary>
        /// The web cam texture to mat helper.
        /// </summary>
        WebCamTextureToMatHelper webCamTextureToMatHelper;

        /// <summary>
        /// The face landmark detector.
        /// </summary>
        FaceLandmarkDetector faceLandmarkDetector;

        /// <summary>
        /// The live2DModel.
        /// </summary>
        public Live2DModel live2DModel;

        /// <summary>
        /// The frontal face parameter.
        /// </summary>
        FrontalFaceParam frontalFaceParam;

        /// <summary>
        /// The shape_predictor_68_face_landmarks_dat_filepath.
        /// </summary>
        private string shape_predictor_68_face_landmarks_dat_filepath;

        /// <summary>
        /// Model file path.
        /// </summary>
        private string moc_filepath;
        private string physics_filepath;
        private string pose_filepath;
        private string[] texture_filepath = new string[6];

        /// <summary>
        /// 相关矩阵的计算.
        /// </summary>
        //F_K 状态转移矩阵
        Matrix F_K = new Matrix(6, 6, "F_K");
        //P_K 后验估计误差协方差矩阵，度量估计值的精确程度
        Matrix P_K = new Matrix(6, 6, "errorCovPsot");
        //P_k+1
        Matrix P_front = new Matrix(6, 6, "errorCovPsot");
        //H_K 观测矩阵
        Matrix H_K = new Matrix(3, 6, "H_K");
        //Q_K 过程噪声
        Matrix Q_K = new Matrix(6, 6, "Q_K");
        //R_K 观测噪声
        Matrix R_K = new Matrix(3, 3, "R_K");
        //Z_K 测量值
        Matrix Z_K = new Matrix(3, 1, "Z_K");
        //X_K 
        Matrix X_K = new Matrix(6, 1, "state");
        //X_K+1 
        Matrix X_front = new Matrix(6, 1, "state");
        //eye
        Matrix eye = new Matrix(6, 6, "state");


        // Use this for initialization
        void Start()
        {
            startBtn = GameObject.Find("StartButton");
            finishBtn = GameObject.Find("FinishButton");
            startBtn.SetActive(true);
            finishBtn.SetActive(false);

            webCamTextureToMatHelper = gameObject.GetComponent<WebCamTextureToMatHelper>();

#if UNITY_WEBGL && !UNITY_EDITOR
            webCamTextureToMatHelper.flipHorizontal = true;
            StartCoroutine(getFilePathCoroutine());
#else
            // FaceLandmark model filepath
            shape_predictor_68_face_landmarks_dat_filepath = DlibFaceLandmarkDetector.Utils.getFilePath("shape_predictor_68_face_landmarks.dat");

            // Load Texture filepath
            LoadTexture();
            
            Run();
#endif
        }

        private void LoadTexture()
        {
            // Load model filepath
            switch (selectedTexture)
            {
                case WhichTexture.noir_santa:
                    {
                        moc_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.moc.bytes");
                        physics_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.physics.json");
                        pose_filepath = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.pose.json");
                        for (int i = 0; i < texture_filepath.Length; i++)
                        {
                            texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("noir_santa/noir_santa.1024/texture_0" + i + ".png");
                        }
                        break;
                    }
                case WhichTexture.uni:
                    {
                        moc_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.moc.bytes");
                        physics_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.physics.json");
                        pose_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.pose.json");
                        for (int i = 0; i < texture_filepath.Length; i++)
                        {
                            texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("uni/uni.1024/texture_0" + i + ".png");
                        }
                        break;
                    }
                default:
                    {
                        moc_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.moc.bytes");
                        physics_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.physics.json");
                        pose_filepath = OpenCVForUnity.Utils.getFilePath("uni/uni.pose.json");
                        for (int i = 0; i < texture_filepath.Length; i++)
                        {
                            texture_filepath[i] = OpenCVForUnity.Utils.getFilePath("uni/uni.1024/texture_0" + i + ".png");
                        }
                        break;
                    }
            }

            live2DModel.textureFiles = new Texture2D[texture_filepath.Length];
            for (int i = 0; i < texture_filepath.Length; i++)
            {
                if (string.IsNullOrEmpty(texture_filepath[i]))
                    continue;

                Texture2D tex = new Texture2D(2, 2);
                tex.LoadImage(File.ReadAllBytes(texture_filepath[i]));
                live2DModel.textureFiles[i] = tex;
            }
            if (!string.IsNullOrEmpty(moc_filepath))
                live2DModel.setMocFileFromBytes(File.ReadAllBytes(moc_filepath));
            if (!string.IsNullOrEmpty(physics_filepath))
                live2DModel.setPhysicsFileFromBytes(File.ReadAllBytes(physics_filepath));
            if (!string.IsNullOrEmpty(pose_filepath))
                live2DModel.setPoseFileFromBytes(File.ReadAllBytes(pose_filepath));

        }

        private void Run ()
        {
            Debug.Log ("Run");

            faceLandmarkDetector = new FaceLandmarkDetector (shape_predictor_68_face_landmarks_dat_filepath);
            frontalFaceParam = new FrontalFaceParam ();

            // Use the front camera to Init
            webCamTextureToMatHelper.Init(null, webCamTextureToMatHelper.requestWidth, webCamTextureToMatHelper.requestHeight, !webCamTextureToMatHelper.requestIsFrontFacing);

            //// Default initialization
            //webCamTextureToMatHelper.Init();
            //webCamTextureToMatHelper.Init(null, 320, 240, false);
        }

        /// <summary>
        /// Raises the web cam texture to mat helper inited event.
        /// </summary>
        public void OnWebCamTextureToMatHelperInited ()
        {
            Debug.Log ("OnWebCamTextureToMatHelperInited");

            Mat webCamTextureMat = webCamTextureToMatHelper.GetMat ();
            
            gameObject.transform.localScale = new Vector3 (webCamTextureMat.cols (), webCamTextureMat.rows (), 1);

            Debug.Log ("Screen.width " + Screen.width + " Screen.height " + Screen.height + " Screen.orientation " + Screen.orientation);

            float width = gameObject.transform.localScale.x;
            float height = gameObject.transform.localScale.y;

            float widthScale = (float)Screen.width / width;
            float heightScale = (float)Screen.height / height;
            if (widthScale < heightScale) {
                Camera.main.orthographicSize = (width * (float)Screen.height / (float)Screen.width) / 2;
            } else {
                Camera.main.orthographicSize = height / 2;
            }

            if (live2DModel != null) {
                // Set live2DModel localScale
                live2DModel.transform.localScale = new Vector3 (Camera.main.orthographicSize * modelScale, Camera.main.orthographicSize * modelScale, 1);
            }
            
        }

        /// <summary>
        /// Raises the web cam texture to mat helper disposed event.
        /// </summary>
        public void OnWebCamTextureToMatHelperDisposed ()
        {
            Debug.Log ("OnWebCamTextureToMatHelperDisposed");

        }

        /// <summary>
        /// Raises the web cam texture to mat helper error occurred event.
        /// </summary>
        public void OnWebCamTextureToMatHelperErrorOccurred ()
        {
            Debug.Log("OnWebCamTextureToMatHelperErrorOccurred");
        }

        // Update is called once per frame
        void Update ()
        {
            if (Input.GetKeyDown(KeyCode.Escape))
            {
#if UNITY_5_3 || UNITY_5_3_OR_NEWER
                SceneManager.LoadScene("FacemojiStart");
#else
                Application.LoadLevel("FacemojiStart");
#endif
            }

            if (webCamTextureToMatHelper.IsPlaying () && webCamTextureToMatHelper.DidUpdateThisFrame ()) {

                Mat rgbaMat = webCamTextureToMatHelper.GetMat();

                OpenCVForUnityUtils.SetImage(faceLandmarkDetector, rgbaMat);
                //获得特征匹配矩形

                List<UnityEngine.Rect> detectResult = faceLandmarkDetector.Detect ();

                foreach (var rect in detectResult) {
                    //获得68个采样点
                    List<Vector2> points = faceLandmarkDetector.DetectLandmark (rect);

                    if (points.Count > 0) {
                        // angle
                        Vector3 angles = frontalFaceParam.getFrontalFaceAngle(points);
                        //初始状态
                        double[,] originState = X_K.Detail;
                        originState[0, 0] = angles.x;
                        originState[1, 0] = angles.y;
                        originState[2, 0] = angles.z;
                        originState[3, 0] = 0;
                        originState[4, 0] = 0;
                        originState[5, 0] = 0;
                        //eye
                        double[,] unit = eye.Detail;
                        unit[0, 0] = 1;
                        unit[0, 1] = 0;
                        unit[0, 2] = 0;
                        unit[0, 3] = 0;
                        unit[0, 4] = 0;
                        unit[0, 5] = 0;
                        unit[1, 0] = 0;
                        unit[1, 1] = 1;
                        unit[1, 2] = 0;
                        unit[1, 3] = 0;
                        unit[1, 4] = 0;
                        unit[1, 5] = 0;
                        unit[2, 0] = 0;
                        unit[2, 1] = 0;
                        unit[2, 2] = 1;
                        unit[2, 3] = 0;
                        unit[2, 4] = 0;
                        unit[2, 5] = 0;
                        unit[3, 0] = 0;
                        unit[3, 1] = 0;
                        unit[3, 2] = 0;
                        unit[3, 3] = 1;
                        unit[3, 4] = 0;
                        unit[3, 5] = 0;
                        unit[4, 0] = 0;
                        unit[4, 1] = 0;
                        unit[4, 2] = 0;
                        unit[4, 3] = 0;
                        unit[4, 4] = 1;
                        unit[4, 5] = 0;
                        unit[5, 0] = 0;
                        unit[5, 1] = 0;
                        unit[5, 2] = 0;
                        unit[5, 3] = 0;
                        unit[5, 4] = 0;
                        unit[5, 5] = 1;


                        //F_K（F_k）
                        double[,] stateC = F_K.Detail;
                        stateC[0, 0] = 1;
                        stateC[0, 1] = 0;
                        stateC[0, 2] = 0;
                        stateC[0, 3] = 1;
                        stateC[0, 4] = 0;
                        stateC[0, 5] = 0;
                        stateC[1, 0] = 0;
                        stateC[1, 1] = 1;
                        stateC[1, 2] = 0;
                        stateC[1, 3] = 0;
                        stateC[1, 4] = 1;
                        stateC[1, 5] = 0;
                        stateC[2, 0] = 0;
                        stateC[2, 1] = 0;
                        stateC[2, 2] = 1;
                        stateC[2, 3] = 0;
                        stateC[2, 4] = 0;
                        stateC[2, 5] = 1;
                        stateC[3, 0] = 0;
                        stateC[3, 1] = 0;
                        stateC[3, 2] = 0;
                        stateC[3, 3] = 1;
                        stateC[3, 4] = 0;
                        stateC[3, 5] = 0;
                        stateC[4, 0] = 0;
                        stateC[4, 1] = 0;
                        stateC[4, 2] = 0;
                        stateC[4, 3] = 0;
                        stateC[4, 4] = 1;
                        stateC[4, 5] = 0;
                        stateC[5, 0] = 0;
                        stateC[5, 1] = 0;
                        stateC[5, 2] = 0;
                        stateC[5, 3] = 0;
                        stateC[5, 4] = 0;
                        stateC[5, 5] = 1;

                        //P_K(P_k)
                        double[,] errCovPost = P_K.Detail;
                        errCovPost[0, 0] = 0.1;
                        errCovPost[0, 1] = 0;
                        errCovPost[0, 2] = 0;
                        errCovPost[0, 3] = 0;
                        errCovPost[0, 4] = 0;
                        errCovPost[0, 5] = 0;
                        errCovPost[1, 0] = 0;
                        errCovPost[1, 1] = 0.1;
                        errCovPost[1, 2] = 0;
                        errCovPost[1, 3] = 0;
                        errCovPost[1, 4] = 0;
                        errCovPost[1, 5] = 0;
                        errCovPost[2, 0] = 0;
                        errCovPost[2, 1] = 0;
                        errCovPost[2, 2] = 0.1;
                        errCovPost[2, 3] = 0;
                        errCovPost[2, 4] = 0;
                        errCovPost[2, 5] = 0;
                        errCovPost[3, 0] = 0;
                        errCovPost[3, 1] = 0;
                        errCovPost[3, 2] = 0;
                        errCovPost[3, 3] = 0.1;
                        errCovPost[3, 4] = 0;
                        errCovPost[3, 5] = 0;
                        errCovPost[4, 0] = 0;
                        errCovPost[4, 1] = 0;
                        errCovPost[4, 2] = 0;
                        errCovPost[4, 3] = 0;
                        errCovPost[4, 4] = 0.1;
                        errCovPost[4, 5] = 0;
                        errCovPost[5, 0] = 0;
                        errCovPost[5, 1] = 0;
                        errCovPost[5, 2] = 0;
                        errCovPost[5, 3] = 0;
                        errCovPost[5, 4] = 0;
                        errCovPost[5, 5] = 0.1;
                        //Debug.Log(MatrixOperator.MatrixPrint(P_K));

                        //H_K(H_K)
                        double[,] measureMat = H_K.Detail;
                        measureMat[0, 0] = 1;
                        measureMat[0, 1] = 0;
                        measureMat[0, 2] = 0;
                        measureMat[0, 3] = 0;
                        measureMat[0, 4] = 0;
                        measureMat[0, 5] = 0;
                        measureMat[1, 0] = 0;
                        measureMat[1, 1] = 1;
                        measureMat[1, 2] = 0;
                        measureMat[1, 3] = 0;
                        measureMat[1, 4] = 0;
                        measureMat[1, 5] = 0;
                        measureMat[2, 0] = 0;
                        measureMat[2, 1] = 0;
                        measureMat[2, 2] = 1;
                        measureMat[2, 3] = 0;
                        measureMat[2, 4] = 0;
                        measureMat[2, 5] = 0;

                        //Q_K(Q_K)
                        double[,] processNoiseCov = Q_K.Detail;
                        processNoiseCov[0, 0] = 1;
                        processNoiseCov[0, 1] = 0;
                        processNoiseCov[0, 2] = 0;
                        processNoiseCov[0, 3] = 0;
                        processNoiseCov[0, 4] = 0;
                        processNoiseCov[0, 5] = 0;
                        processNoiseCov[1, 0] = 0;
                        processNoiseCov[1, 1] = 1;
                        processNoiseCov[1, 2] = 0;
                        processNoiseCov[1, 3] = 0;
                        processNoiseCov[1, 4] = 0;
                        processNoiseCov[1, 5] = 0;
                        processNoiseCov[2, 0] = 0;
                        processNoiseCov[2, 1] = 0;
                        processNoiseCov[2, 2] = 1;
                        processNoiseCov[2, 3] = 0;
                        processNoiseCov[2, 4] = 0;
                        processNoiseCov[2, 5] = 0;
                        processNoiseCov[3, 0] = 0;
                        processNoiseCov[3, 1] = 0;
                        processNoiseCov[3, 2] = 0;
                        processNoiseCov[3, 3] = 1;
                        processNoiseCov[3, 4] = 0;
                        processNoiseCov[3, 5] = 0;
                        processNoiseCov[4, 0] = 0;
                        processNoiseCov[4, 1] = 0;
                        processNoiseCov[4, 2] = 0;
                        processNoiseCov[4, 3] = 0;
                        processNoiseCov[4, 4] = 1;
                        processNoiseCov[4, 5] = 0;
                        processNoiseCov[5, 0] = 0;
                        processNoiseCov[5, 1] = 0;
                        processNoiseCov[5, 2] = 0;
                        processNoiseCov[5, 3] = 0;
                        processNoiseCov[5, 4] = 0;
                        processNoiseCov[5, 5] = 1;

                        //R_K(R_K)
                        double[,] measureNoiseCov = R_K.Detail;
                        measureNoiseCov[0, 0] = 0.03;
                        measureNoiseCov[0, 1] = 0;
                        measureNoiseCov[0, 2] = 0;
                        measureNoiseCov[1, 0] = 0;
                        measureNoiseCov[1, 1] = 0.03;
                        measureNoiseCov[1, 2] = 0;
                        measureNoiseCov[2, 0] = 0;
                        measureNoiseCov[2, 1] = 0;
                        measureNoiseCov[2, 2] = 0.03;

                        double[] Arr = new double[] { angles.x, angles.y, angles.z};
                        //X_0 初始状态
                        double[,] measure = Z_K.Detail;
                        measure[0, 0] = Arr[0];
                        measure[1, 0] = Arr[1];
                        measure[2, 0] = Arr[2];

                        //首先计算推测值
                        Matrix X_front = MatrixOperator.MatrixMulti(F_K, X_K);
                        //然后计算推测值与真实值之间的误差协方差矩阵
                        Matrix P_front = MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(F_K, P_K), MatrixOperator.MatrixTrans(F_K)), Q_K);
                        //再计算卡尔曼增益
                        Matrix Kg = MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(P_front, MatrixOperator.MatrixTrans(H_K)), MatrixOperator.MatrixInv(MatrixOperator.MatrixAdd(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(H_K, P_front), MatrixOperator.MatrixTrans(H_K)), R_K)));

                        //再得到估计值
                        Matrix X_front_ = MatrixOperator.MatrixAdd(X_front, MatrixOperator.MatrixMulti(Kg, MatrixOperator.MatrixSub(Z_K, MatrixOperator.MatrixMulti(H_K, X_front))));
                        //最后计算估计值与真实值之间的误差协方差矩阵，为下次递推做准备
                        Matrix P_front_ = MatrixOperator.MatrixMulti(MatrixOperator.MatrixSub(eye, MatrixOperator.MatrixMulti(Kg, H_K)), P_front);
                        X_K = X_front_;
                        P_K = P_front_;
                        double[,] c = X_K.Detail;
                        double aa = c[0, 0];
                        double bb = c[1,0];
                        double  cc = c[2, 0];
                        angles.x = (int)aa;
                        angles.y = (int)bb;
                        angles.z = (int)cc;

                        float rotateX = (angles.x > 180) ? angles.x - 360 : angles.x;
                        float rotateY = (angles.y > 180) ? angles.y - 360 : angles.y;
                        float rotateZ = (angles.z > 180) ? angles.z - 360 : angles.z;
                        // Coordinate transformation
                        //live2DModel.PARAM_ANGLE.Set(-rotateY, rotateX, -rotateZ);
                        live2DModel.PARAM_ANGLE.Set(rotateX, rotateY, rotateZ);

                        // Make sure your line of sight is always facing the camera
                        // eye_ball_X
                        live2DModel.PARAM_EYE_BALL_X = rotateX / 60f;
                        // eye_ball_Y
                        live2DModel.PARAM_EYE_BALL_Y = rotateY / 60f - 0.25f;

                        live2DModelUpdate (points);

                        break;
                    }
                }
            }

        }

        private void live2DModelUpdate (List<Vector2> points)
        {

            if (live2DModel != null) {


                //live2DModel.PARAM_ANGLE.Set(-Roundf(rotateY, 0.5f), Roundf(rotateX, 0.5f), -Roundf(rotateZ, 0.5f));

                // brow_L_Y
                float brow_L_Y = getRaitoOfBROW_L_Y(points);
                // Keep three decimal places to reduce the vibration
                live2DModel.PARAM_BROW_L_Y = Roundf(brow_L_Y, 1000f);
                //live2DModel.PARAM_BROW_L_Y = (float)Math.Round(brow_L_Y, 2);

                // brow_R_Y
                float brow_R_Y = getRaitoOfBROW_R_Y(points);
                // Keep three decimal places to reduce the vibration
                live2DModel.PARAM_BROW_R_Y = Roundf(brow_R_Y, 1000f);
                //live2DModel.PARAM_BROW_R_Y = (float)Math.Round(brow_R_Y, 2);

                // eye_open_L
                float eyeOpen_L = getRaitoOfEyeOpen_L (points);
                if (eyeOpen_L > 0.6f && eyeOpen_L < 1.1f)
                    eyeOpen_L = 1;
                else if (eyeOpen_L >= 1.1f && brow_L_Y >= 0 )
                    eyeOpen_L = 2;
                else if (eyeOpen_L <= 0.6f)
                    eyeOpen_L = 0;
                live2DModel.PARAM_EYE_L_OPEN = eyeOpen_L;

                // eye_open_R
                float eyeOpen_R = getRaitoOfEyeOpen_R (points);
                if (eyeOpen_R > 0.6f && eyeOpen_R < 1.1f)
                    eyeOpen_R = 1;
                else if (eyeOpen_R >= 1.1f && brow_R_Y >= 0)
                    eyeOpen_R = 2;
                else if (eyeOpen_R < 0.6f)
                    eyeOpen_R = 0;
                live2DModel.PARAM_EYE_R_OPEN = eyeOpen_R;



                // mouth_open
                float mouthOpen = getRaitoOfMouthOpen_Y (points);
                if (mouthOpen < 0.6f)
                    mouthOpen = 0;
                live2DModel.PARAM_MOUTH_OPEN_Y = mouthOpen;

                // mouth_size
                float mouthSize = getRaitoOfMouthSize (points);
                live2DModel.PARAM_MOUTH_SIZE = mouthSize;

            }
        }

        // Keep decimal places to reduce the vibration
        private float Roundf(float f, float multiple)
        {
            if (multiple == 0)
                return f;
            int i = (int)(f * multiple);
            return i / multiple;
        }

        // Calculate the degree of eye opening
        private float getRaitoOfEyeOpen_L (List<Vector2> points)
        {
            if (points.Count != 68)
                throw new ArgumentNullException ("Invalid landmark_points.");

            return Mathf.Clamp (Mathf.Abs (points [43].y - points [47].y) / (Mathf.Abs (points [43].x - points [44].x) * 0.75f), -0.1f, 2.0f);
        }

        private float getRaitoOfEyeOpen_R (List<Vector2> points)
        {
            if (points.Count != 68)
                throw new ArgumentNullException ("Invalid landmark_points.");

            return Mathf.Clamp (Mathf.Abs (points [38].y - points [40].y) / (Mathf.Abs (points [37].x - points [38].x) * 0.75f), -0.1f, 2.0f);
        }

        // Eyebrows move up and down
        private float getRaitoOfBROW_L_Y (List<Vector2> points)
        {
            if (points.Count != 68)
                throw new ArgumentNullException ("Invalid landmark_points.");

            //float y = Mathf.Ceil(Mathf.Abs(points[24].y - points[27].y)) / Mathf.Abs (points [27].y - points [29].y);
            float y = Mathf.Abs(points[24].y - points[27].y) / Mathf.Abs(points[27].y - points[29].y);
            y -= 1;
            y *= 4f;
            
            return Mathf.Clamp (y, -1.0f, 1.0f);
        }

        private float getRaitoOfBROW_R_Y (List<Vector2> points)
        {
            if (points.Count != 68)
                throw new ArgumentNullException ("Invalid landmark_points.");

            //float y = Mathf.Ceil(Mathf.Abs(points[19].y - points[27].y)) / Mathf.Abs(points[27].y - points[29].y);
            float y = Mathf.Abs(Mathf.Sqrt(Mathf.Pow(points[19].y - points[27].y,2)+Mathf.Pow(points[19].x - points[27].x,2))) / Mathf.Abs(Mathf.Sqrt(Mathf.Pow(points[27].y - points[29].y,2)+Mathf.Pow(points[27].x - points[29].x,2))) ;
            Debug.Log("y是："+y);
            y -= 1;
            y *= 4f;

            return Mathf.Clamp (y, -1.0f, 1.0f);
        }

        // Calculate the degree of mouth opening
        private float getRaitoOfMouthOpen_Y (List<Vector2> points)
        {
            if (points.Count != 68)
                throw new ArgumentNullException ("Invalid landmark_points.");

            return (float)((points[57].y - points[51].y) / (points[64].x - points[60].x) - 0.2);

            //return Mathf.Clamp01 (Mathf.Abs (points [62].y - points [66].y) / (Mathf.Abs (points [51].y - points [62].y) + Mathf.Abs (points [66].y - points [57].y)));
        }

        // Calculate the width of the mouth
        private float getRaitoOfMouthSize (List<Vector2> points)
        {
            if (points.Count != 68)
                throw new ArgumentNullException ("Invalid landmark_points.");

            float size = Mathf.Abs (points [48].x - points [54].x) / (Mathf.Abs (points [31].x - points [35].x) * 1.8f);
            size -= 1;
            size *= 4f;

            return Mathf.Clamp (size, -1.0f, 1.0f);
        }

        /// <summary>
        /// Raises the disable event.
        /// </summary>
        void OnDisable ()
        {
            if(webCamTextureToMatHelper != null) webCamTextureToMatHelper.Dispose ();

            if(faceLandmarkDetector != null) faceLandmarkDetector.Dispose ();

            if(frontalFaceParam != null) frontalFaceParam.Dispose ();
        }

        public void OnBackButton()
        {
#if UNITY_5_3 || UNITY_5_3_OR_NEWER
            SceneManager.LoadScene("FacemojiStart");
#else
            Application.LoadLevel("FacemojiStart");
#endif
        }

        /// <summary>
        /// Raises the change camera button event.
        /// </summary>
        public void OnChangeCameraButton ()
        {
            webCamTextureToMatHelper.Init (null, webCamTextureToMatHelper.requestWidth, webCamTextureToMatHelper.requestHeight, !webCamTextureToMatHelper.requestIsFrontFacing);
        }

        public void OnStartButton()
        {
            startBtn.SetActive(false);
            if (!Record.isRecording)
            {
                //Start recording
                Record.isRecording = true;
                finishBtn.SetActive(true);
            }
        }

        public void OnFinishButton()
        {
            finishBtn.SetActive(false);
            if (Record.isRecording)
            {
                //Finish recording
                Record.isRecording = false;
                startBtn.SetActive(true);
            }
        }

        public void OnShizukuButton()
        {
            if(selectedTexture != WhichTexture.noir_santa)
            {
                selectedTexture = WhichTexture.noir_santa;
                LoadTexture();
            }
        }

        public void OnHaruButton()
        {
            if (selectedTexture != WhichTexture.uni)
            {
                selectedTexture = WhichTexture.uni;
                LoadTexture();
            }
        }

    }
}