????????歡迎來(lái)到本博客????????
??博主優(yōu)勢(shì)：??????博客內(nèi)容盡量做到思維縝密，邏輯清晰，為了方便讀者。
??座右銘：行百里者，半于九十。
??????本文目錄如下：??????
目錄
??1 概述
??2 運(yùn)行結(jié)果
?2.1?算例1
?
2.2 算例2?
2.3?算例3
??3?參考文獻(xiàn)
??4 Matlab代碼及數(shù)據(jù)
??1 概述
EKF 是卡爾曼濾波器在非線性系統(tǒng)中的應(yīng)用的推廣延伸，其離散非線性系統(tǒng)的狀態(tài)和測(cè)量方程表示為：
EKF 原理如圖 1 所示。
EKF 主要包含時(shí)間更新（預(yù)測(cè)）與測(cè)量更新（校正）兩個(gè)階段。 時(shí)間更新包含以下部分：
?
卡爾曼濾波器法原理由射影定理推導(dǎo)而來(lái)，能在線性高斯模型的情況下對(duì)目標(biāo)狀態(tài)做出最優(yōu)估計(jì)，但實(shí)際系統(tǒng)多為非線性系統(tǒng)[83]。為解決非線性系統(tǒng)濾波問(wèn)題，常用處理方法是將其看作一個(gè)近似的線性濾波問(wèn)題。目前應(yīng)用較多的是 EKF，其核心思想是在濾波值處將非線性函數(shù)和進(jìn)行一階泰勒級(jí)數(shù)展開(kāi)，并忽略其高階項(xiàng)，得到局部線性化模型，然后再應(yīng)用 KF 進(jìn)行濾波估計(jì)。
??2 運(yùn)行結(jié)果
?2.1?算例1
?
?
2.2 算例2?
?
2.3?算例3
?
?
部分代碼：
N = 20; % number of time steps
dt = 1; % time between time steps
M = 100; % number of Monte-Carlo runs
sig_acc_true = [0.3; 0.3; 0.3]; % true value of standard deviation of accelerometer noise
sig_gps_true = [3; 3; 3; 0.03; 0.03; 0.03]; % true value of standard deviation of GPS noise
sig_acc = [0.3; 0.3; 0.3]; % user input of standard deviation of accelerometer noise
sig_gps = [3; 3; 3; 0.03; 0.03; 0.03]; % user input of standard deviation of GPS noise
Q = [diag(0.25*dt^4*sig_acc.^2), zeros(3); zeros(3), diag(dt^2*sig_acc.^2)]; % process noise covariance matrix
R = [diag(sig_gps(1:3).^2), zeros(3); zeros(3), diag(sig_gps(4:6).^2)]; % measurement noise covariance matrix
F = [eye(3), eye(3)*dt; zeros(3), eye(3)]; % state transition matrix
B = [0.5*eye(3)*dt^2; eye(3)*dt]; % control-input matrix
H = eye(6); % measurement matrix
%% true trajectory
x_true = zeros(6,N+1); % true state
a_true = zeros(3,N); ? % true acceleration
x_true(:,1) = [0; 0; 0; 5; 5; 0]; % initial true state
for k = 2:1:N+1
? ? x_true(:,k) = F*x_true(:,k-1) + B*a_true(:,k-1);
end
%% Kalman filter simulation
res_x_est = zeros(6,N+1,M); % Monte-Carlo estimates
res_x_err = zeros(6,N+1,M); % Monte-Carlo estimate errors
P_diag = zeros(6,N+1); % diagonal term of error covariance matrix
% filtering
for m = 1:1:M
? ? % initial guess
? ? x_est(:,1) = [2; -2; 0; 5; 5.1; 0.1];
? ? P = [eye(3)*4^2, zeros(3); zeros(3), eye(3)*0.4^2];
? ? P_diag(:,1) = diag(P);
? ? for k = 2:1:N+1
? ? ? ??
? ? ? ? %%% Prediction
? ? ? ? % obtain acceleration output
? ? ? ? u = a_true(:,k-1) + normrnd(0, sig_acc_true);
? ? ? ??
? ? ? ? % predicted state estimate
? ? ? ? x_est(:,k) = F*x_est(:,k-1) + B*u;
? ? ? ??
? ? ? ? % predicted error covariance
? ? ? ? P = F*P*F' + Q;
? ? ? ??
? ? ? ? %%% Update
? ? ? ? % obtain measurement
? ? ? ? z = x_true(:,k) + normrnd(0, sig_gps_true);
? ? ? ??
? ? ? ? % measurement residual
??3?參考文獻(xiàn)
部分理論來(lái)源于網(wǎng)絡(luò)，如有侵權(quán)請(qǐng)聯(lián)系刪除。
[1]彭劍,劉東文.改進(jìn)擴(kuò)展卡爾曼濾波器的PMSM參數(shù)辨識(shí)[J].現(xiàn)代信息科技,2023,7(10):66-69.DOI:10.19850/j.cnki.2096-4706.2023.10.017.文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-500861.html
[2]廖楷嫻. 改進(jìn)擴(kuò)展卡爾曼濾波器的永磁同步風(fēng)力發(fā)電機(jī)參數(shù)辨識(shí)[D].湖南工業(yè)大學(xué),2022.DOI:10.27730/d.cnki.ghngy.2022.000263.文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-500861.html
??4 Matlab代碼及數(shù)據(jù)
到了這里，關(guān)于【狀態(tài)估計(jì)】基于卡爾曼濾波器和擴(kuò)展卡爾曼濾波器用于 INS/GNSS 導(dǎo)航、目標(biāo)跟蹤和地形參考導(dǎo)航研究（Matlab代碼實(shí)現(xiàn)）的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
