目錄
1.算法描述
2.仿真效果預(yù)覽
3.MATLAB核心程序
4.完整MATLAB
1.算法描述
? ? ? ?卡爾曼濾波是一種高效率的遞歸濾波器(自回歸濾波器)，它能夠從一系列的不完全包含噪聲的測(cè)量中，估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)。這種濾波方法以它的發(fā)明者魯?shù)婪颉·卡爾曼（Rudolf E. Kalman）命名?？柭畛跆岢龅臑V波理論只適用于線性系統(tǒng)。Bucy，Sunahara等人提出并研究了擴(kuò)展卡爾曼濾波(EKF)，將卡爾曼濾波理論進(jìn)一步應(yīng)用到非線性領(lǐng)域。
? ? ? ?擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）是標(biāo)準(zhǔn)卡爾曼濾波在非線性情形下的一種擴(kuò)展形式，EKF算法是將非線性函數(shù)進(jìn)行泰勒展開，省略高階項(xiàng)，保留展開項(xiàng)的一階項(xiàng)，以此來(lái)實(shí)現(xiàn)非線性函數(shù)線性化，最后通過卡爾曼濾波算法近似計(jì)算系統(tǒng)的狀態(tài)估計(jì)值和方差估計(jì)值,對(duì)信號(hào)進(jìn)行濾波。
? ? ? ?擴(kuò)展卡爾曼濾波EKF的狀態(tài)轉(zhuǎn)移方程和觀測(cè)方程為： ??
?EKF和KF的區(qū)別如下：
? ? 姿態(tài)解算就是通過融合傳感器數(shù)據(jù)解算出姿態(tài)角。姿態(tài)角是俯仰角（pitch）、滾轉(zhuǎn)角（roll）和偏航角（yaw）的合稱，此文分別使用 α , β , γ \alpha, \beta, \gammaα,β,γ 表示。
俯仰角
? ? ? ?俯仰角是無(wú)人機(jī)機(jī)體系 x 軸與水平面夾角，也即機(jī)體系與航向系 x 軸的夾角，機(jī)頭上仰為正，范圍 α ∈ [ ? π / 2 , π / 2 ] \alpha \in [-\pi/2, \pi/2]α∈[?π/2,π/2]。
滾轉(zhuǎn)角
? ? ? 滾轉(zhuǎn)角是無(wú)人機(jī)機(jī)體系 y 軸與水平面夾角，也即機(jī)體系與航向系 y 軸的夾角，機(jī)身左升右降為正，范圍 β ∈ [ ? π , π ] \beta \in [-\pi, \pi]β∈[?π,π]。
偏航角
? ? ? ?偏航角是無(wú)人機(jī)機(jī)體系 x 軸在水平面投影與地球系 x 軸（正北方）的夾角，也即航向系 x 軸與地球系 x 軸夾角。俯視機(jī)身，順時(shí)針方向（往東）角度遞增，逆時(shí)針方向角度遞減，范圍 γ ∈ [ ? π , π ] \gamma \in [-\pi, \pi]γ∈[?π,π]。
? ? ? 可見，引入航向系之后可以更加方便地定義姿態(tài)角。注意與下面的歐拉角作對(duì)比，歐拉角和姿態(tài)角不是同樣概念，這也是這里使用 α , β , γ \alpha, \beta, \gammaα,β,γ 而不是更常見的 θ , ? , ψ \theta, \phi, \psiθ,?,ψ 的原因，后者用于表示歐拉角。
2.仿真效果預(yù)覽
matlab2022a仿真結(jié)果如下：
?
?
?
?
文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-422786.html
3.MATLAB核心程序
%常系數(shù)
L= 0.3875; %單位(m)
Ix = 0.05887; %單位(kg·m^2)
Iy = 0.05887;
Iz = 0.13151;
g = 9.81; %單位(N/kg)
%動(dòng)力學(xué)方程的常系數(shù)
a1 = -(Iy - Iz)/Ix;
a2 = -(Iz - Ix)/Iy;
a3 = -(Ix - Iy)/Iz;
b1 = L/Ix;
b2 = L/Iy;
b3 = 1/Iz;
Ts = 0.1; %采樣時(shí)間
t = 5; %仿真時(shí)間
len = fix(t/Ts); %仿真步數(shù)
n = 6; %狀態(tài)維度
w = 0.1; %過程標(biāo)準(zhǔn)差
v = 0.5; %測(cè)量標(biāo)準(zhǔn)差
Q = w^2*eye(n); %過程方差
R = v^2; %測(cè)量值的方差
h=@(x)[x(2);x(4);x(6)]; %測(cè)量方程
s=[1;2;3;3;2;1]; %初始狀態(tài)
x=s+w*randn(6,1); %初始化狀態(tài)
P = eye(6); %初始化協(xié)方差矩陣
xV = zeros(6,len); %EKF估計(jì)值
sV = zeros(6,len); %真實(shí)值
zV = zeros(3,len); %測(cè)量值
for k=1:len
%隨機(jī)賦值控制量
u2 = 0.1*randn(1,1);
u3 = 0.1*randn(1,1);
u4 = 0.1*randn(1,1);
z = h(s) + v*randn;
sV(:,k)= s; %實(shí)際狀態(tài)
zV(:,k) = z; %狀態(tài)測(cè)量值
%狀態(tài)方程
f=@(x)[x(1)+Ts*x(2);
(a1*x(4)*x(6) +b1*u2)*Ts+x(2);
x(3)+Ts*x(4);
(a2*x(2)*x(6) +b2*u3)*Ts+x(4);
x(5)+Ts*x(6);
(a3*x(2)*x(4) +b3*u4)*Ts+x(6);];
%一步預(yù)測(cè)，同時(shí)計(jì)算f的雅可比矩陣A
[x1,A]=jaccsd(f,x);
%過程方差預(yù)測(cè)
P=A*P*A'+Q;
%狀態(tài)預(yù)測(cè)，同時(shí)計(jì)算h的雅可比矩陣H
[z1,H]=jaccsd(h,x1);
%計(jì)算卡爾曼增益
K=P*H'/(H*P*H'+R);
%狀態(tài)EKF估計(jì)值
x=x1+K*(z-z1);
%協(xié)方差更新
P=P-K*H*P;
xV(:,k) = x;
%更新狀態(tài)
s = f(s) + w*randn(6,1);
end
%俯仰角、滾轉(zhuǎn)角、偏航角度值
for k=1:2:5
figure(); hold on;
plot(sV(k,:),'-.'); %畫出真實(shí)值
plot(xV(k,:)) %畫出最優(yōu)估計(jì)值
plot(abs(sV(k,:)-xV(k,:)), '--'); %畫出誤差值
legend('真實(shí)狀態(tài)', 'EKF最優(yōu)估計(jì)估計(jì)值', '誤差值');
end
A1284.完整MATLAB
V文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-422786.html
到了這里，關(guān)于基于EKF的四旋翼無(wú)人機(jī)姿態(tài)估計(jì)matlab仿真的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
