Conversion of C code to matlab code
3 ビュー (過去 30 日間)
古いコメントを表示
I have a piece of code (arduino code) and I need to change it into MATLAB code. And I donot know how to do that. Can somebody please guide me.
This is the code.
oid KalmanFilterInit() {
Q_GPSHeading1 = 0.001f;
Q_GPSHeadingBias = 0.003f;
R_CompassMeasure = 0.03f;
GPSHeading1 = 0.0f;
GPSHeadingBias = 0.0f;
P[0][0] = 0.01f;
P[0][1] = 0.01f;
P[1][0] = 0.01f;
P[1][1] = 0.01f;
}
void KalmanFilter() {
float GPSTimePrev = millis();
float dt = millis() - GPSTimePrev ;
//Updating estimatio error covariance & projecting the error covariance ahead
P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + GPSHeading1);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += GPSHeadingBias * dt;
//Calculating the Kalman Gain
float S = P[0][0] + R_CompassMeasure; //(estimating the error)
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
//Update estimation into the measurement zk
float KalmanY = CompassHeading - GPSHeading;
GPSHeading1 += K[0] * KalmanY;
GPSHeadingBias += K[1] * KalmanY;
//Calculating the estimation error covariance
float P00_Prime = P[0][0];
float P01_Prime = P[0][1];
P[0][0] -= K[0] * P00_Prime;
P[0][1] -= K[0] * P01_Prime;
P[1][0] -= K[1] * P00_Prime;
P[1][1] -= K[1] * P01_Prime;
AbsGPSHeading1 = abs (GPSHeading1);
return AbsGPSHeading1;
}
0 件のコメント
回答 (1 件)
参考
カテゴリ
Help Center および File Exchange で Arduino Hardware についてさらに検索
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!