最近為了項目，同事讓我幫他做一個硬件版的kalman濾波器，實現對設備的kalman濾波，以驗證他的理論算法。

猶豫了好幾天，用dsp吧，我的kalman濾波算法比較簡單，有點大材小用。剛好手里有一塊arm調試版，也裝了wince系統，就準備在.net環境下編一個kalman濾波器。

雖說學的是導航專業，對kalman濾波應該比較熟悉，可是當時學的就不好，所有學的東西都還給導師了。（導師您不會看到這篇文章吧！看來不能放在首頁上！）

于是，只能在網上找一些相關資料。感覺現在變的懶了，總是喜歡在別人的代碼上改來改去，不愿意去思考了。算了，反正就這么一次。罪過罪過！

國內的資料對于matlab的算法比較多了，網上隨便down，但是基于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 classclass 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;}}#endregionpublic 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的類庫。

轉載于:https://www.cnblogs.com/MobileBo/archive/2010/09/07/1820831.html

總結

以上是生活随笔為你收集整理的【C#】【引用加原创】C#实现kalman滤波的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。