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

時間 2019-11-20

原文原文鏈接

一、角度和弧度之間的轉換公式？
設角度爲 angle，弧度爲 radian
radian = angle * pi / 180;
angle = radian * 180 / pi;
因此在matlab中常常設置一個參數，用於角度與弧度之間的轉換：deg_rad=0.01745329252e0;算法

二、注意下面角度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)來代入公式計算三軸姿態角。數組

三、在濾波的過程當中，要明確濾波時間和採樣頻率；dom

四、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

五、函數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那這裏就是表示標準差了spa

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

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

七、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');%標註

八、EKF的matlab實現： blog

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-');

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

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

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

其中，X(k) 是 k 時刻目標的狀態向量，V(k)是過程噪聲，它是具備均值爲零、方差矩陣爲 Q(k) 的高斯噪聲向量，即
　

Q(k)是狀態轉移矩陣， G(k)是過程噪聲增益矩陣。

二.傳感器的測量（觀測）方程

傳感器的通用觀測方程爲

　
這裏， Z(k+1)是傳感器在 k+1 時刻的觀測向量，觀測噪聲 W(k+1) 是具備零均值和正定協方差矩陣 R(k+1) 的高斯分佈測量噪聲向量，即

三.初始狀態的描述

初始狀態 X(0) 是高斯的，具備均值 X(0|0) 和協方差，即

四.Kalman濾波算法

狀態估計的一步預測方程爲

一步預測的協方差爲

預測的觀測向量爲

觀測向量的預測偏差協方差爲

新息或量測殘差爲

濾波器增益爲

Kalman濾波算法的狀態更新方程爲

濾波偏差協方差的更新方程爲

% Kalman濾波技術

A=1;                                        % 狀態轉移矩陣 Φ(k)
H=0.2;                                      % 觀測矩陣 H(k)
X(1)=0;                                     % 目標的狀態向量 X(k)
% V(1)=0;                                   % 過程噪聲 V(k)
Y(1)=1;                                     % 一步預測x(k)的更新 X(k+1|k+1)
P(1)=10;                                    % 一步預測的協方差 P(k)
N=200;
V=randn(1,N);                               % 模擬產生過程噪聲(高斯分佈的隨機噪聲)
w=randn(1,N);                               % 模擬產生測量噪聲

for k=2:N

    X(k) = A * X(k-1)+V(k-1);               % 狀態方程:X(k+1)=Φ(k)X(k)+G(k)V(k),其中G(k)=1

end

Q=std(V)^2;                                 % W(k)的協方差,std()函數用於計算標準誤差  
R=std(w)^2;                                 % V(k)的協方差 covariance

Z=H*X+w;                                    % 觀測方程:Z(k+1)=H(k+1)X(k+1)+W(k+1),Z(k+1)是k+1時刻的觀測值

for t=2:N

    P(t) = A * P(t-1)+Q;                    % 一步預測的協方差 P(k+1|k)   

    S(t) = H.^2 * P(t)+R;                   % 觀測向量的預測偏差協方差 S(k+1)

    K(t) = H * P(t)/S(t);                   % 卡爾曼濾波器增益 K(k+1) 

    v(t) = Z(t) - ( A * H * Y(t-1) );       % 新息/量測殘差 v(k+1)

    Y(t)=A * Y(t-1) + K(t) * v(t);          % 狀態更新方程 X(k+1|k+1)=X(k+1|k)+K(k+1)*v(k+1)

    P(t)=(1-H * K(t)) * P(t);               % 偏差協方差的更新方程: P(k+1|k+1)=(I-K(k+1)*H(k+1))*P(k+1|k)
end


t=1:N;
plot(t,Y,'r',t,Z,'g',t,X,'b');              % 紅色線最優化估算結果濾波後的值，%綠色線觀測值，藍色線預測值
legend('Kalman濾波結果','觀測值','預測值');

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

但在實際工做中，咱們的系統幾乎都是非線性的，因此如何解決非線性系統的濾波問題呢，這就是咱們要講的EKF（擴展卡爾曼濾波），它的原理很簡單，就是在估計狀態的地方進行線性化，線性化的方法也很簡單，只須要進行泰勒的一階展開就行。固然咱們也要說一下缺點，由於咱們選擇的線性化方法，因此只能達到二階的精度（UKF能夠達到四階的精度），因此要求咱們的系統是弱線性化的。其實UKF也是對非線性系統的卡爾曼濾波，主要本人對其線性化方法（UT變換）不是很理解，等考完試再說。
首先，系統的狀態方程和觀測方程以下：
${\textbf {x}}_{{k}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},{\textbf {w}}_{{k}})$
${\textbf {z}}_{{k}}=h({\textbf {x}}_{{k}},{\textbf {v}}_{{k}})$
咱們知道，在更新偏差協方差矩陣的時候，不能直接用f和h的，因此咱們要進行泰勒展開，也就是要求雅克比矩陣。再利用線性狀況下的卡爾曼濾波進行計算更新。
預測：
${\hat {{\textbf {x}}}}_{{k|k-1}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},0)$
${\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}$
利用雅克比矩陣進行更新模型：

${\textbf {F}}_{{k}}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k-1|k-1}},{\textbf {u}}_{{k}}}}$
${\textbf {H}}_{{k}}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k|k-1}}}}$
更新：
${\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-h({\hat {{\textbf {x}}}}_{{k|k-1}},0)$
${\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}$
${\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}$
${\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}$
${\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}$
下面我會展現一個目標追蹤的EKF例子：

%擴展Kalman濾波在目標跟蹤中的應用
  
%function EKF_For_TargetTracking
  
clc;clear;
 
T=1;%雷達掃描週期
 
N=60/T;%總的採樣次數
   
X=zeros(4,N);%目標真實位置、速度
 
X(:,1)=[-100,2,200,20];%目標初始位置、速度
 
Z=zeros(1,N);%傳感器對位置的觀測
  
delta_w=1e-3;%若是增大這個參數，目標的真實軌跡就是曲線了
   
Q=delta_w*diag([0.5,1]);%過程噪聲方差
   
G=[T^2/2,0;T,0;0,T^2/2;0,T];%過程噪聲驅動矩陣
  
R=5;%觀測噪聲方差
   
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%狀態轉移矩陣
  
x0=200;%觀測站的位置
 
y0=300;

Xstation=[x0,y0];
    
for t=2:N
 
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目標真實軌跡
 
end
   
 
  
for t=1:N
     
    Z(t)=Dist(X(:,t),Xstation)+sqrtm(R)*randn;%觀測值

end
    
%EKF
    
Xekf=zeros(4,N);
    
Xekf(:,1)=X(:,1);%Kalman濾波的狀態初始化
    
P0=eye(4);%偏差協方差矩陣的初始化
    
for i=2:N
    
    Xn=F*Xekf(:,i-1);%一步預測
    
    P1=F*P0*F'+G*Q*G';%預測偏差協方差
    
    dd=Dist(Xn,Xstation);%觀測預測
     
    %求解雅克比矩陣H
    
    H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%泰勒展開的一階近似
    
    K=P1*H'*inv(H*P1*H'+R);%卡爾曼最優增益
    
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);%狀態更新
    
    P0=(eye(4)-K*H)*P1;%濾波偏差協方差更新
    
end
   
%偏差分析
  
for i=1:N
    
    Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%
   
end
   
%畫圖
   
figure
   
hold on;box on;
    
plot(X(1,:),X(3,:),'-k');%真實軌跡
     
plot(Xekf(1,:),Xekf(3,:),'-r');%擴展Kalman濾波軌跡
    
legend(' real trajectory','EKF trajectory');
    
xlabel('X-axis  X/m');
    
ylabel('Y-axis Y/m');
  
 
    
figure
  
hold on ;box on;
   
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
    
xlabel('Time /s');
    
ylabel('Position estimation bias   /m');
    
 
    
%子函數 求歐氏距離
     
function d=Dist(X1,X2);
    
if length(X2)<=2
     
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
   
else
    
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
    
end
  
 
     
%子函數 求歐氏距離
     
function d=Dist(X1,X2);
     
if length(X2)<=2
     
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
    
else
    
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
     
end

相關標籤/搜索

每日一句

每一个你不满意的现在，都有一个你没有努力的曾经。