[C \ #] [원본 인용] C \ # kalman 필터 실현

8464 단어 man
최근 프로젝트 를 위해 동료 들 은 나 에 게 하드웨어 판 kalman 필 터 를 만들어 서 장치 에 대한 kalman 필 터 를 실현 하여 그의 이론 알고리즘 을 검증 하 라 고 했다.
며칠 동안 망 설 였 습 니 다. dsp 를 사용 하 세 요. 제 kalman 필터 산법 은 비교적 간단 하고 큰 인재 가 적 습 니 다.마침 손 에 arm 디 버 깅 버 전도 있 고 wince 시스템 도 설치 되 어 있 으 며. net 환경 에서 kalman 필 터 를 만 들 려 고 합 니 다.
비록 네 비게 이 션 학 과 를 배 웠 기 때문에 kalman 필터 에 익숙 해 야 하지만 그 당시 에 잘 배우 지 못 해서 모든 것 을 지도 교수 님 께 돌려 드 렸 습 니 다.(스승 님, 설마 이 글 을 보 시 는 건 아니 겠 죠? 홈 페이지 에 올 려 놓 으 면 안 될 것 같 습 니 다!)
그래서 인터넷 에서 관련 자 료 를 찾 을 수 밖 에 없 었 다.지금 은 게 을 러 진 것 같 아서 다른 사람의 코드 를 고 치 는 것 을 좋아 하고 생각 하고 싶 지 않 습 니 다.됐어, 어차피 한번 만.죄, 죄!
국내의 자 료 는 matlab 에 대한 알고리즘 이 비교적 많아 서 인터넷 에서 마음대로 다운 되 었 지만 C \ # 를 바탕 으로 하 는 것 이 비교적 적 습 니 다. 다행히 제 검색 능력 이 그리 나 쁘 지 않 아서 관련 자 료 를 찾 게 되 었 습 니 다.
블 로 그 를 인용 한 주 소 는: http://blog.csdn.net/csdnbao/archive/2009/09/24/4590519.aspx
문장 이 전체 알고리즘 을 다 썼 으 니 나 도 함께 붙 여 놓 자!
 
 
using System;

using System.Collections.Generic;

using System.Text;



namespace SimTransfer

{

    public class KalmanFacade

    {

        #region inner class

        class KalmanFilter

        {

            int MP;                     /* number of measurement vector dimensions */

            int DP;                     /* number of state vector dimensions */

            int CP;                     /* number of control vector dimensions */



            public Matrix state_pre;           /* predicted state (x'(k)):

                                        x(k)=A*x(k-1)+B*u(k) */

            public Matrix state_post;          /* corrected state (x(k)):

                                        x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */

            public Matrix transition_matrix;   /* state transition matrix (A) */

            public Matrix control_matrix;      /* control matrix (B)

                                       (it is not used if there is no control)*/

            public Matrix measurement_matrix;  /* measurement matrix (H) */

            public Matrix process_noise_cov;   /* process noise covariance matrix (Q) */

            public Matrix measurement_noise_cov; /* measurement noise covariance matrix (R) */

            public Matrix error_cov_pre;       /* priori error estimate covariance matrix (P'(k)):

                                        P'(k)=A*P(k-1)*At + Q)*/

            Matrix gain;                /* Kalman gain matrix (K(k)):

                                        K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/

            Matrix error_cov_post;      /* posteriori error estimate covariance matrix (P(k)):

                                        P(k)=(I-K(k)*H)*P'(k) */

            Matrix temp1;               /* temporary matrices */

            Matrix temp2;

            Matrix temp3;

            Matrix temp4;

            Matrix temp5;



            public KalmanFilter()

            {

                MP = 1;

                DP = 2;

                CP = 0;

                state_pre = new Matrix(DP, 1);

                state_pre.Zero();

                state_post = new Matrix(DP, 1);

                state_post.Zero();

                transition_matrix = new Matrix(DP, DP);

                transition_matrix.SetIdentity(1.0);

                transition_matrix[0, 1] = 1;

                process_noise_cov = new Matrix(DP, DP);

                process_noise_cov.SetIdentity(1.0);

                measurement_matrix = new Matrix(MP, DP);

                measurement_matrix.SetIdentity(1.0);

                measurement_noise_cov = new Matrix(MP, MP);

                measurement_noise_cov.SetIdentity(1.0);

                error_cov_pre = new Matrix(DP, DP);

                error_cov_post = new Matrix(DP, DP);

                error_cov_post.SetIdentity(1);

                gain = new Matrix(DP, MP);

                if (CP > 0)

                {

                    control_matrix = new Matrix(DP, CP);

                    control_matrix.Zero();

                }

                //

                temp1 = new Matrix(DP, DP);

                temp2 = new Matrix(MP, DP);

                temp3 = new Matrix(MP, MP);

                temp4 = new Matrix(MP, DP);

                temp5 = new Matrix(MP, 1);

            }



            public Matrix Predict()

            {

                state_pre = transition_matrix.Multiply(state_post);

                //if (CP>0)

                //{

                //    control_matrix

                //}

                temp1 = transition_matrix.Multiply(error_cov_post);

                Matrix at = transition_matrix.Transpose();

                error_cov_pre = temp1.Multiply(at).Add(process_noise_cov);



                Matrix result = new Matrix(state_pre);

                return result;

            }



            public Matrix Correct(Matrix measurement)

            {

                temp2 = measurement_matrix.Multiply(error_cov_pre);

                Matrix ht = measurement_matrix.Transpose();

                temp3 = temp2.Multiply(ht).Add(measurement_noise_cov);

                temp3.InvertSsgj();

                temp4 = temp3.Multiply(temp2);

                gain = temp4.Transpose();



                temp5 = measurement.Subtract(measurement_matrix.Multiply(state_pre));

                state_post = gain.Multiply(temp5).Add(state_pre);



                error_cov_post = error_cov_pre.Subtract(gain.Multiply(temp2));



                Matrix result = new Matrix(state_post);

                return result;

            }



            public Matrix AutoPredict(Matrix measurement)

            {

                Matrix result = Predict();



                Correct(measurement);



                return result;

            }

        }

        #endregion



        public KalmanFacade(int valueItem)

        {

            if (valueItem<=0)

            {

                throw new Exception("not enough value items!");

            }

            kmfilter = new KalmanFilter[valueItem];

            Random rand = new Random(1001);

            for (int i = 0; i < valueItem; i++ )

            {

                kmfilter[i] = new KalmanFilter();

                kmfilter[i].state_post[0, 0] = rand.Next(10);

                kmfilter[i].state_post[1, 0] = rand.Next(10);

                //

                kmfilter[i].process_noise_cov.SetIdentity(1e-5);

                kmfilter[i].measurement_noise_cov.SetIdentity(1e-1);

            }

        }



        private KalmanFilter[] kmfilter = null; 

        

        public bool GetValue(double[] inValue, ref double[] outValue)

        {

            if (inValue.Length != kmfilter.Length || outValue.Length != kmfilter.Length)

            {

                return false;

            }



            Matrix[] measures = new Matrix[kmfilter.Length];

            

            for (int i = 0; i < kmfilter.Length; i++ )

            {

                measures[i] = new Matrix();

                measures[i][0, 0] = inValue[i];

                outValue[i] = kmfilter[i].AutoPredict(measures[i])[0, 0];

            }



            return true;

        }

    }



}



//==========test=============



            SimTransfer.KalmanFacade kalman = new SimTransfer.KalmanFacade(1);

            Random rand = new Random(1001);

            System.IO.StreamWriter dataFile = new System.IO.StreamWriter("D:\\test.csv");

            

            for (int x = 0; x < 2000; x++ )

            {

                double y = 100 * Math.Sin((2.0 * Math.PI / (float)200) * x);

                

                double noise = 20 * Math.Sin((40.0 * Math.PI / (float)200) * x) + 40 * (rand.NextDouble() - 0.5);



                double[] z_k = new double[1];

                z_k[0] = y + noise;

                double[] y_k = new double[1];

                kalman.GetValue(z_k, ref y_k);



                dataFile.WriteLine(y.ToString()  + "," + z_k[0].ToString()  + "," + y_k[0].ToString());

            }



            dataFile.Close();

            MessageBox.Show("OK!");




 
원본 코드 는 매우 상세 하지만 주석 이 비교적 적 습 니 다. 보아하니 나 는 프로그램 을 다시 번역 해 야 할 것 같 습 니 다!
요 이틀 동안 주석 을 다시 써 라!!!
또 하 나 는 Matrix 의 라 이브 러 리 다.

좋은 웹페이지 즐겨찾기