MPU6050 + 一階互補濾波+二階互補濾波+卡爾曼濾波 +波形比較

阿新 • • 發佈：2019-01-19

1、卡爾曼濾波函式

void Kalman_Filter_X(float Accel, float Gyro)
{
	Angle_X_Final += (Gyro - Q_bias_x) * dt;       //先驗估計

	Pdot[0] = Q_angle - PP[0][1] - PP[1][0];     //Pk-先驗估計誤差協方差的微分
	Pdot[1] = -PP[1][1];
	Pdot[2] = -PP[1][1];
	Pdot[3] = Q_gyro;

	PP[0][0] += Pdot[0] * dt;   //Pk-先驗估計誤差協方差的微分的積分=先驗估計誤差協方差
	PP[0][1] += Pdot[1] * dt;   
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;

	Angle_err_x = Accel - Angle_X_Final;	//zk-先驗估計

	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];

	E = R_angle + C_0 * PCt_0;

	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;

	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	PP[0][0] -= K_0 * t_0;	   //後驗估計誤差協方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;

	Angle_X_Final += K_0 * Angle_err_x;	//後驗估計
	Q_bias_x += K_1 * Angle_err_x;	  //後驗估計
	Gyro_x = Gyro - Q_bias_x;	 //輸出值（後驗估計）的微分=角速度
}

2、一、二階互補濾波函式

void yijiehubu_R(float angle_m, float gyro_m)
{
	float K1 =0.1;
	angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
	A_R = angle_R;
}

void erjiehubu_P(float angle_m, float gyro_m)
{
	float K = 0.05;
	float y1;
	float x1 = (angle_m - angle_P) * K * K;
	y1 = y1 + x1 * dt;
	float x2 = y1 + 2 * K *(angle_m - angle_P) + gyro_m;
	angle_P = angle_P + x2 * dt;
	A_P = angle_P;
}


float K2 =0.2;
float x1,x2,y1;
float angle2;

void Erjielvbo(float angle_m,float gyro_m)
{
	x1=(angle_m-angle2)*(1-K2)*(1-K2);
	y1=y1+x1*dt;
	x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m;
	angle2=angle2+ x2*dt;
	A2_P = angle2;
}

3、一階互補濾波與二階互補濾波比較

4、原始波形與卡爾曼濾波比較

5、卡爾曼濾波與一階互補濾波比較

6、二階互補濾波與卡爾曼濾波比較

7、簡單總結

- 卡爾曼濾波與二階互補濾波波形基本一致
- 一階互補濾波較二階互補濾波與卡爾曼濾波效果較差
- 三種波形均無滯後現象

MPU6050 + 一階互補濾波+二階互補濾波+卡爾曼濾波 +波形比較

1、卡爾曼濾波函式 void Kalman_Filter_X(float Accel, float Gyro) { Angle_X_Final += (Gyro - Q_bias_x) * dt; //先驗估計 Pdot[0] = Q_angle -

一階互補濾波，二階互補濾波，卡爾曼濾波

一階互補 // a=tau / (tau + loop time) // newAngle = angle measured with atan2 using the accelerometer //加速度感測器輸出值 // newRate = angle measured

卡爾曼濾波 -- 從推導到應用(一) 到 (二)

/************************************************************************************************************************************

卡爾曼濾波原理及應用（一）

出於科研需要，打算開始學習卡爾曼濾波（Kalmam Filter）。很早之前就聽說過卡爾曼濾波,但一直沒能深入學習，這次終於有機會了，哈哈。 1.卡爾曼濾波的發展過程卡爾曼濾波的本質屬於"估計"範疇.先介紹下估計，所謂“估計”問題，就是對收到隨機干擾和隨機測量誤差作用的物理系統，按照某種效

卡爾曼濾波二—MATLAB程式設計

這是原理介紹後的第二篇：MALTAB程式設計。這是原文連結：https://blog.csdn.net/heyijia0327/article/details/17667341 這裡主要講了卡爾曼濾波。等下次空閒的時候我會把EKF和UKF做一個簡單的介紹。畢竟現實生活中的很多場景都是非線性的。

卡爾曼濾波（一）

舉個例子，上一時刻房間溫度最優值為26度，由於溫度變化緩慢，則可以根據經驗估計本次溫度也為26度，即四式右邊第一項，而此時溫度計讀數為28度，那麼本時刻溫度應該是多少？不知道溫度計精度的情況下，可以對兩個資料求平均，為27度，即T=26+0.5*（28-26）=27。這裡0.5就是本例子中的修正係數，那麼假如

卡爾曼濾波原理(二)：擴充套件卡爾曼

1、理論部分上一篇介紹了線性卡爾曼濾波器，當系統為線性高斯模型時，濾波器能給出最優的估計，但是實際系統總是存在不同程度的非線性，如平方、三角關係、開方等。對於非線性系統，可以採用的一種方法是通過線性化方法將非線性系統轉換為近似的線性系統，即為EKF

無人駕駛汽車系統入門（一）——卡爾曼濾波與目標追蹤

前言：隨著深度學習近幾年來的突破性進展，無人駕駛汽車也在這些年開始不斷向商用化推進。很顯然，無人駕駛汽車已經不是遙不可及的“未來技術”了，未來10年

卡爾曼濾波初探（一）

卡爾曼濾波初探基於時域的線性模型預測這裡先給出幾個概念（初看的時候很多部落格都沒有這方面說明，若你看到下面懵逼的時候，不妨上來再看看？）預測：就是根據已有的①經驗、②公式、③以及上一個時間（）下檢測物件的狀態的最優估計等資訊，從而得到一個對下一個

無人駕駛汽車系統入門（二）——高階運動模型和擴充套件卡爾曼濾波

前言：上一篇文章的最後我們提到卡爾曼濾波存在著一個非常大的侷限性——它僅能對線性的處理模型和測量模型進行精確的估計，在非線性的場景中並不能達到最優的估計效果。所以之前為了保證我們的處理模型是線性的，我們上一節中使用了恆定速度模型，然後將估計目標的加減速用處理噪聲來表示，這一模

初學者的卡爾曼濾波——擴充套件卡爾曼濾波（一）

簡介　　已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著重於將卡爾曼濾波應用於非線性系統。　　為什麼學界要這麼熱衷於將卡爾曼濾波器用於非線性系統呢？因為卡爾曼濾波器從一開

慣性導航——擴充套件卡爾曼濾波（一）

對於無人機的慣性導航系統，系統的狀態方程是非線性的，根據擴充套件卡爾曼濾波方程： Predict x^k|k−1Pk|k−1=f(x^k−1|k−1,uk−1)=Fk−1Pk−1|k−1FT

卡爾曼濾波 -- 從推導到應用(一)

前言卡爾曼濾波器是在估計線性系統狀態的過程中，以最小均方誤差為目的而推匯出的幾個遞推數學等式,也可以從貝葉斯推斷的角度來推導。本文將分為兩部分：第一部分，結合例子

mpu6050姿態解算與卡爾曼濾波（1）數學

定義地理座標系n系：x軸指向東，y軸指向北，z軸指向天。在mpu6050晶片上定義載體座標系b系。那麼b系的姿態就是指n系與b系相對的旋轉關係，即如何由n系旋轉到b系。描述這種旋轉關係通常使用尤拉角(ψ,θ,γ)T，姿態矩陣T(3x3)，四元數Q=(q0，q

數據會有一些波動和噪點采用卡爾曼濾波算法進行

卡爾曼濾波實現多項式擬合Matlab

nom and kalman ffi 樣本矩陣協方差數組 fontsize %%%%%%%%%%%%%Q3:多項式系數估計%%%%%%%%%%%%%%%% %%%%%%%%%%2016/07/21%%%%%%%%%%%%%%%%%%% clc;clear;

卡爾曼濾波的一個簡單demo：恒定加速度模型

obi vtt efk rtp end atp cee cdn bs4 p { margin-bottom: 0.1in; direction: ltr; color: rgb(0, 0, 10); line-height: 120%; text-align: left }

卡爾曼濾波3

比較應該想要不能模型不同隨著而已風景假設你去一個風景區旅遊時候，種下一棵果樹，但顯然從果樹苗到果實不是一天兩天能完成對。需要慢慢長高。而你又不可能經常去看，但又想知道樹的高度，哪怎麽辦？所以要解決的問題是：如何正確估計一棵果樹的高度？我們想要知道的果

卡爾曼濾波2

推導概率精確情況多維結果 width tex 為什麽作者：肖暢鏈接：https://www.zhihu.com/question/23971601/answer/46480923來源：知乎著作權歸作者所有。商業轉載請聯系作者獲得授權，非商業轉載請註明出處。

卡爾曼濾波

nbsp 應用一周 log -1 現在 richtext http 一是假設我養了一只豬：一周前，這只豬的體重是46±0.5kg。註意，在這裏我用了±0.5，表示其實我對這只豬一周前的體重並不是那麽確定的，也就是說，46k

MPU6050 + 一階互補濾波+二階互補濾波+卡爾曼濾波 +波形比較

相關推薦