2. 程式人生 > >MATLAB-卡爾曼濾波簡單運用示例

MATLAB-卡爾曼濾波簡單運用示例

阿新 • • 發佈：2018-06-08
south noi 增加 jacob mage 影響 方差 sqrt mark

1、角度和弧度之間的轉換公式？
設角度為 angle，弧度為 radian
radian = angle * pi / 180;
angle = radian * 180 / pi;
所以在matlab中經常設置一個參數，用於角度與弧度之間的轉換：deg_rad=0.01745329252e0;

2、註意下面角度Angint的表示方法：
Angint=[0,10,0]*deg_rad；
則：Angint(0) = 0;Angint(1) = 0.0175;Angint(2) = 0;
這種表示方法可以在四元數中應用：
例如:
q=[cos(Angint(3)/2)*sin(Angint(2)/2)*cos(Angint(1)/2)+sin(Angint(3)/2)*cos(Angint(2)/2)*sin(Angint(1)/2)
cos(Angint(3)/2)*cos(Angint(2)/2)*sin(Angint(1)/2)-sin(Angint(3)/2)*sin(Angint(2)/2)*cos(Angint(1)/2)
-sin(Angint(3)/2)*cos(Angint(2)/2)*cos(Angint(1)/2)+cos(Angint(3)/2)*sin(Angint(2)/2)*sin(Angint(1)/2)
cos(Angint(3)/2)*cos(Angint(2)/2)*cos(Angint(1)/2)+sin(Angint(3)/2)*sin(Angint(2)/2)*sin(Angint(1)/2)];
可以用q(0)、q(1)、q(2)、q(3)來代入公式計算三軸姿態角。

3、在濾波的過程中，要明確濾波時間和采樣頻率；

4、IMU數據仿真分析：
（1）先模擬加速度計和陀螺儀的真實輸出：
[ Angle,Wibb,Fb ] = imu_true_out( tt,ynong,T );%tt=tt+TT;TT=1/f為時間間隔
註意：加速度計的輸出要進行坐標轉換： ao=Cnb*([0,0,g]’，
其中：Cnb

技術分享圖片

如果你要在加速度計的輸出上添加一個隨機幹擾(可模擬非重力加速度幹擾)，可以使用函數awgn(); %Add white Gaussian noise to a signal.
ao=Cnb*([0,0,g]’+[0,awgn(0,ynong),0]’); %如果在指點的時間內添加這種幹擾，可以加一個if函數
（

2）模擬加速度計和陀螺儀的誤差：
[Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(tt,TT,Gyro_b,Gyro_r,Gyro_wg,Acc_r);

function [Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(t,T,Gyro_b,Gyro_r,Gyro_wg,Acc_r)

g=9.7803698;         %重力加速度    （單位：米/秒/秒）
Wie=7.292115147e-5;  %地球自轉角速度（單位：弧度/秒）
deg_rad=0.01745329252e0;% Transfer from
 
 angle degree to rad

Da_bias=[0.001; 0.001; 0.001]*g;  %加速度零偏（應與非隨機性誤差中的加速度零偏保持一致）

Ta=1800.0; %加速度一階馬爾可夫過程相關時間
Tg=3600.0; %陀螺一階馬爾可夫過程相關時間

%%%%%%%%%%%%%%%%%%隨機性誤差%%%%%%%%%%%%%%%
if( t==0 )
  Acc_r=Da_bias.*randn(3,1); %加速度一階馬爾可夫過程1.0e-4g

  Gyro_b=0.01*deg_rad/3600.0*randn(3,1); %隨機常數0.1(deg/h)
  Gyro_r=0.01*deg_rad/3600.0*randn(3,1); %陀螺一階馬爾可夫過程0.1(deg/h)
  Gyro_wg=0.01*deg_rad/3600.0*randn(3,1);%陀螺白噪聲0.1(deg/h)
else  
  Acc_wa=sqrt(2*T/Ta)*Da_bias.*randn(3,1);%加速度一階馬爾可夫過程白噪聲
  Acc_r=exp(-1.0*T/Ta)*Acc_r; %加速度一階馬爾可夫過程

  Gyro_wr=0.01*sqrt(2*T/Tg)*deg_rad/3600.0*randn(3,1);%陀螺一階馬爾可夫過程白噪聲0.1(deg/h)
  Gyro_r=exp(-1.0*T/Tg)*Gyro_r+Gyro_wr;%陀螺一階馬爾可夫過程0.1(deg/h)
  Gyro_wg=0.01*deg_rad/3600.0*randn(3,1);%陀螺白噪聲0.1(deg/h)
end

然後再在while(1)中將真實值+誤差值按時間序列存儲在數組中以備用，如下：

while tt<=T;
    [ Angle,Wibb,Fb ] = imu_true_out( tt,ynong,T );
    [Gyro_b,Gyro_r,Gyro_wg,Acc_r]=imu_err_random(tt,TT,Gyro_b,Gyro_r,Gyro_wg,Acc_r);
    Ture_Angle(:,kk)=Angle/deg_rad;%模擬輸出的三軸姿態角
    gyro(:,kk)=Wibb+Gyro_b+Gyro_r+Gyro_wg;%模擬輸出的陀螺儀輸出
    acc(:,kk)=Fb+Acc_r;%模擬輸出的加速度計輸出
    tt=tt+TT;
    kk=kk+1;%這裏TT=1/f=1/100為采樣時間間隔，f為采樣頻率，T為采樣時間，文中設置為30s
end

5、函數randn的意思：
R = randn(3,1)；%產生3行1列的隨機R矩陣，R矩陣滿足方差為1，均值為0；註意這裏不是說這三個數的方差為1，均值為0，而是每次運行randn時，當運行的量足夠大時，所有的R(0)的方差為1，均值為0，R(1)、R(2)同理。
例如1. randn(1) ；%生成一個隨機數，要想滿足方差為1，均值為0，也必須運行足夠多的次數
例如2. x = .6 + sqrt(0.1) * randn(1)；%產生均值為0.6，方差為0.1的一個5*5的隨機數；sqrt(0.1)對0.1進行開方，直接寫0.1那這裏就是表示標準差了

randi([m,n],a,b) %[m,n] 的 a*b 隨機數

6、axis()函數的用法：
axis([xmin xmax ymin ymax])；%定義x軸和y軸之間的間距

7、set()函數的用法：
plot(t,acc(2,:),‘linewidth‘,1.3); 
set(gcf,‘Position‘,[100 100 400 300]);  %這裏的100是指生成的圖片距離左下角的距離，400和300分別表示長和寬 
axis([0 T -4.9 2.0]); 
set(gca, ‘YTick‘, [-4.9 -2.4 -0.1 2.0]) 
set(gca,‘fontname‘,‘宋體‘,‘fontsize‘,10); %用於改變坐標軸坐標大小，10代表坐標大小； 
xlabel(‘時間/s‘,‘fontname‘,‘宋體‘,‘fontsize‘,10);%用於改變x軸標註的文字和大小；
ylabel(‘加速度計數據/（g）‘,‘fontname‘,‘宋體‘,‘fontsize‘,10);%用於改變y軸標註的文字和大小；
legend(‘accy‘);%標註
技術分享圖片 




 8、EKF的matlab實現： 


 


kalman的前提條件：


1）線性系統 
2）系統噪聲和測量噪聲服從高斯分布



卡爾曼最初提出的濾波理論只適用於線性系統,Bucy，Sunahara等人提出並研究了擴展卡爾曼濾波(Extended Kalman Filter，簡稱EKF)，將卡爾曼濾波理論進一步應用到非線性領域。EKF的基本思想是將非線性系統線性化，然後進行卡爾曼濾波，因此EKF是一種次優濾波。其後，多種二階廣義卡爾曼濾波方法的提出及應用進一步提高了卡爾曼濾波對非線性系統的估計性能。二階濾波方法考慮了Taylor級數展開的二次項，因此減少了由於 線性化所引起的 估計誤差，但大大增加了運算量，因此在實際中反而沒有一階EKF應用廣泛。 
在狀態方程或測量方程為非線性時，通常采用擴展卡爾曼濾波(EKF)。EKF對非線性函數的Taylor展開式進行一階線性化截斷，忽略其余高階項，從而將非線性問題轉化為線性，可以將卡爾曼線性濾波算法應用於非線性系統中。這樣一來，解決了非線性問題。EKF雖然應用於非線性狀態估計系統中已經得到了學術界認可並為人廣泛使用，然而該種方法也帶來了兩個缺點，其一是當強非線性時EKF違背局部線性假設，Taylor展開式中被忽略的高階項帶來大的 誤差時，EKF算法可能會使濾波發散；另外，由於EKF在線性化處理時需要用雅克比(Jacobian)矩陣，其繁瑣的計算過程導致該方法實現相對困難。所以，在滿足 線性系統、高斯白噪聲、所有隨機變量服從高斯(Gaussian)分布這3個假設條件時，EKF是最小方差準則下的次優濾波器，其性能依賴於局部非線性度。



%---------------------------EKF算法實現------------------------------------
for k=2:kk-1
%---------------不考慮噪聲時，根據狀態方程預測當前值--------------------------
Ag=AntiMatrix(gyro(:,k-1));%計算陀螺儀輸出的反對稱矩陣
Ag=[-Ag,   gyro(:,k-1)
    -gyro(:,k-1)‘, 0];  
Ag=expm(Ag/2/f); %系統一步轉移矩陣

Tg=AntiMatrix(q(1:3,k-1));
Tg=[Tg+eye(3)*q(4,k-1)
    -(q(1:3,k-1))‘];
Tg=-0.5*Tg/f; %計算系統噪聲矩陣

q(:,k)=Ag*q(:,k-1);%計算姿態四元數的狀態估計值

%------------------------計算此時的方差-------------------------------------
p=Ag*p*Ag‘+Tg*Q*Tg‘;
%------------------------計算卡爾曼增益-------------------------------------
r=[0,0,g]‘;
qv=q(1:3,k); %狀態方程四元數矢量部分
qs=q(4,k);   %狀態方程四元數標量部分

%計算量測向量
Hg=2*[qv‘*r*eye(3)+qv*r‘-r*qv‘+qs*AntiMatrix(r),qs*r+AntiMatrix(r)*qv];

Kg=p*Hg‘*(Hg*p*Hg‘+R)^-1;%計算卡爾曼增益

%------------------根據當前量測值，對預測更新--------------------------------
Cnb=(qs^2-qv‘*qv)*eye(3)+2*qv*qv‘-2*qs*AntiMatrix(qv);
h=Cnb*r;
q(:,k)=q(:,k)+Kg*(acc(:,k)-h);

%-----------------------更新估計值方差--------------------------------------
p=(eye(4)-Kg*Hg)*p;

%---------------------------歸一化-----------------------------------------
h(k)=sqrt(q(1,k)^2+q(2,k)^2+q(3,k)^2+q(4,k)^2);
q(:,k)=q(:,k)/h(k);

%----------------------------姿態角計算-------------------------------------
%補償後（-180，180）180會出現奇點,-180度正常,但是不影響,實際上-180和180是重合的

T33=(q(3,k))^2-(q(2,k))^2-(q(1,k))^2+(q(4,k))^2;
T13=2*(q(1,k)*q(3,k)-q(4,k)*q(2,k));
T23=2*(q(2,k)*q(3,k)+q(4,k)*q(1,k));
if T33>0  %根據物理意義不可能出現0
    ANGLE0(1,k)=-180/pi*atan(T13/T33);
else
    ANGLE0(1,k)=180/pi*(-pi*sign(T13)-atan(T13/T33));
end

ANGLE0(2,k)=180/pi*asin(T23); %俯仰角，繞X軸轉動

end




註意此程序的特點，需關註以下問題：





1）如何根據陀螺儀輸出計算系統一步轉移矩陣：

技術分享圖片

 




2）如何根據四元數計算系統的噪聲矩陣：技術分享圖片；


四元數卡爾曼濾波算法的狀態方程（由陀螺儀技術分享圖片輸出計算得到）： 技術分享圖片；



四元數卡爾曼濾波的時間輸出過程如下：技術分享圖片


 


3）如何根據四元數計算系統的量測向量； 
Hg=2*[qv’*r*eye(3)+qv*r’-r*qv’+qs*AntiMatrix(r),qs*r+AntiMatrix(r)*qv]; 


 


4）如何根據四元數計算系統的轉態轉移矩陣； 
Cnb=(qs^2-qv’*qv)*eye(3)+2*qv*qv’-2*qs*AntiMatrix(qv); 
註意對預測值（即四元數）的更新過程： 
q(:,k)=q(:,k)+Kg*(acc(:,k)-h); 
因為是由加速度計構建系統的量測方程的，acc(:,k)表示加速度計的量測值，h=Cnb*r為先驗估計（不考慮誤差，r=[0,0,g]’;），這裏是將地理坐標系轉化為導航坐標系。 
實際上，kalman的5個公式一部分是從狀態方程和量測方程上獲取的。 


 


5）如何根據四元數計算系統的姿態角：技術分享圖片


 


 


6）反對稱矩陣用函數表示AntiMatrix()：     技術分享圖片



function [A] = AntiMatrix(B)
A=[0,-B(3,1),B(2,1)
    B(3,1),0,-B(1,1)
    -B(2,1),B(1,1),0];
end





 ----------------------------------------------------------------------分割線----------------------------------------------------------------------------



Z=(1:100); %觀測值    
noise=randn(1,100); %方差為1的高斯噪聲    
Z=Z+noise;    
    
X=[0; 0]; %狀態    
Sigma = [1 0; 0 1]; %狀態協方差矩陣    
F=[1 1; 0 1]; %狀態轉移矩陣    
Q=[0.0001, 0; 0 0.0001]; %狀態轉移協方差矩陣    
H=[1 0]; %觀測矩陣    
R=1; %觀測噪聲方差    
    
figure;    
hold on;    
    
for i=1:100    
    
  X_ = F*X;    
  Sigma_ = F*Sigma*F‘+Q;    
  K = Sigma_*H‘/(H*Sigma_*H‘+R);    
  X = X_+K*(Z(i)-H*X_);    
  Sigma = (eye(2)-K*H)*Sigma_;    
      
  plot(X(1), X(2), ‘.‘,‘MarkerSize‘,10); %畫點，橫軸表示位置，縱軸表示速度    
end  
  
plot([0,100],[1,1],‘r-‘); 



 
MATLAB-卡爾曼濾波簡單運用示例
   
 
 
 
 

            
  

           

              
              

            

            
相關推薦

			   
            
            
            
 

    


    
    
MATLAB-卡爾曼濾波簡單運用示例

     
 south   noi   增加   jacob   mage   影響   方差   sqrt   mark   1、角度和弧度之間的轉換公式？ 設角度為 angle，弧度為 radian radian = angle * pi / 180; angle = radian * 180 / pi; 所以在ma 



  
 

    


    
    
無跡卡爾曼濾波簡單理解

     
 對於上一篇中的問題：X ∼ N(µ, σ^2 ) , Y = sin(X)要求隨機變數Y的期望和方差。還有一種思路是對X進行取樣，比如取500個取樣點(這些取樣點可以稱為sigma點)，然後求取這些取樣點的期望和方差。當取樣值足夠大時,結果與理論值接近。這種思路的問題顯而易見，當隨機變數維數增大時取樣點的數量 



  
 

    


    
    
【Kalman】卡爾曼濾波Matlab簡單實現

     
 
							
							
							  本節卡爾曼濾波Matlab實現是針對線性系統估計的，僅為簡單模擬。

1.離散時間線性動態系統的狀態方程

  線性系統採用狀態方程、觀測方程及其初始條件來描述。線性離散時間系統的一般狀態方程可描述為 


  其中，X(k) 是 k 時刻目標的狀態向量，V 



  
 

    


    
    
卡爾曼濾波實現多項式擬合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 } 



  
 

    


    
    
卡爾曼濾波MATLAB程式碼實現

     
  
 
 沒有大量的公式推導，個人感覺也沒有必要，我們從小推導過很多公式，試著想想我們還能回憶起幾個？個人認為只需要記住公式的用法，作用，知道有這個公式就可以。用的時候我們可以隨時去查。所以樓主參考網上資料結合一個小例子整理出卡爾曼濾波的MATLAB程式碼實現。看懂這些程式碼我們需要對卡爾曼濾波演算法有基本的 



  
 

    


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

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



  
 

    


    
    
無人駕駛四 擴充套件卡爾曼濾波在目標車輛運動狀態識別中的運用（python程式）

     
 
                

# coding=utf-8

import numpy as np
import numdifftools as nd
import math

dataset = []

# read the measurement data, use 0.0 to stand LI 



  
 

    


    
    
基於matlab模擬對卡爾曼濾波的理解

     
 
								
								            
						
                
clear
clc;
N = 200;           
t = 1:N;
w = randn(1,N);          %噪聲w
x(1) = 0;
for i = 2:N
x(i) = x 



  
 

    


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

     
  



  
 

    


    
    
卡爾曼濾波3

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



  
 

    


    
    
卡爾曼濾波2

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

 



  
 

    


    
    
卡爾曼濾波

     
 nbsp   應用   一周   log   -1   現在   richtext   http   一是    
假設我養了一只豬：

 
一周前，這只豬的體重是46±0.5kg。註意，在這裏我用了±0.5，表示其實我對這只豬一周前的體重並不是那麽確定的，也就是說，46k 



  
 

    


    
    
卡爾曼濾波的原理說明

     
 收集   引入   濾波   div   概率   一個人   pdf   span   net   

在學習卡爾曼濾波器之前，首先看看為什麽叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！卡爾曼全名Ru 



  
 

    


    
    
【概率機器人】3.1 卡爾曼濾波、擴展卡爾曼濾波和無跡卡爾曼濾波

     
 取出   嘗試   bar   return   tar   簡化   exp   回顧   clas   這一章將介紹卡爾曼濾波、擴展卡爾曼濾波以及無跡卡爾曼濾波，並從貝葉斯濾波的角度來進行分析並完成數學推導。如果您對貝葉斯濾波不了解，可以查閱相關書籍或閱讀 【概率機器人 2 遞歸狀態估計】。
這三種濾波方 



  
 

    


    
    
cv2使用卡爾曼濾波（Kalman Filter）捕捉滑鼠運動

     
  
 
 本文主要介紹在cv2中使用Kalman濾波捕捉滑鼠運動。 
 cv2.KalmanFilter(dynamParams=None,#狀態的維度 
 measureParams=None, #測量的維度 
 controlParams=None,#控制的維度 
 type=None)#矩陣的型別 
  



  
 

    


    
    
卡爾曼濾波的理解以及推導過程

     
  
 
 針對的系統為： 
 狀態方程       X(k)=AX(k-1)+Bu(k-1)+W(k-1) 
 測量方程       Z(k)=HX(k)+V(k)  & 



  
 

    


    
    
通俗理解卡爾曼濾波及其演算法實現（帶例項解析）

     
  
 
 1．簡介(Brief Introduction) 
 在學習卡爾曼濾波器之前，首先看看為什麼叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！ 
 卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於 



  
 

    


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

     
  
 
 簡介 
 轉自：http://www.cnblogs.com/ymxiansen/p/5368547.html 
 
 　　已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著 



  
 

    


    
    
詳解卡爾曼濾波

     
  
 
 輕鬆一下看一個例子： 
 一片綠油油的草地上有一條曲折的小徑，通向一棵大樹。一個要求被提出：從起點沿著小徑走到樹下。 “很簡單。” A說，於是他絲毫不差地沿著小徑走到了樹下。 現在，難度被增加了：蒙上眼。 “也不難，我當過特種兵。” B說，於是他歪歪扭扭地走到了樹 ……