基于卡尔曼滤波器、扩展卡尔曼滤波的惯性导航系统/全球导航卫星系统导航、目标跟踪、地形参考导航研究（Matlab代码实现）

 💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

2.1 惯性导航系统/全球导航卫星系统导航

2.2 目标跟踪

2.3 地形参考导航

🎉3 参考文献

🌈4 Matlab代码、数据

---

💥1 概述

摘要
本文提供了关于卡尔曼滤波器和扩展卡尔曼滤波器的类似教程的描述。本章旨在为那些需要向他人教授卡尔曼滤波器的人提供帮助，或者对估计理论没有很强背景的人。在给出状态估计问题定义之后，将介绍滤波算法，并提供支持性示例，以帮助读者轻松理解卡尔曼滤波器的工作原理。给出了在惯性导航系统/全球导航卫星系统（INS/GNSS）导航、目标跟踪和地形参考导航（TRN）方面的实现。在每个示例中，我们讨论如何选择、实现、调整和修改算法以适应实际应用。
关键词：卡尔曼滤波器、扩展卡尔曼滤波器、惯性导航系统/全球导航卫星系统导航、目标跟踪、地形参考导航。

卡尔曼滤波是一种算法，根据随时间观测到的测量值，提供一些未知变量的估计值。卡尔曼滤波在各种应用中展示了其实用性。卡尔曼滤波器具有相对简单的形式，并且需要较少的计算能力。然而，对于不熟悉估计理论的人来说，理解和实现卡尔曼滤波器仍然不容易。虽然存在一些优秀的文献，介绍了卡尔曼滤波器背后的推导和理论，但本章重点关注更实用的角度。

接下来的两章将分别介绍卡尔曼滤波器和扩展卡尔曼滤波器的算法，包括它们的应用。在具有加性高斯噪声的线性模型中，卡尔曼滤波器提供最优估计。全球导航卫星系统（GNSS）导航将作为卡尔曼滤波器的实现示例。扩展卡尔曼滤波器用于非线性问题，如方位角目标跟踪和地形参考导航（TRN）。如何为这些应用实现滤波算法将被详细介绍。

📚2 运行结果

2.1 惯性导航系统/全球导航卫星系统导航

2.2 目标跟踪

 

2.3 地形参考导航

部分代码：

%% settings

N = 20; % number of time steps
dt = 1; % time between time steps
M = 100; % number of Monte-Carlo runs

sig_mea_true = [0.02; 0.02; 1.0]; % true value of standard deviation of measurement noise

sig_pro = [0.5; 0.5; 0.5]; % user input of standard deviation of process noise
sig_mea = [0.02; 0.02; 1.0]; % user input of standard deviation of measurement noise

sig_init = [1; 1; 0; 0; 0; 0]; % standard deviation of initial guess

Q = [zeros(3), zeros(3); zeros(3), diag(sig_pro.^2)]; % process noise covariance matrix
R = diag(sig_mea.^2); % measurement noise covariance matrix

F = [eye(3), eye(3)*dt; zeros(3), eye(3)]; % state transition matrix
B = eye(6); % control-input matrix
u = zeros(6,1); % control vector

H = zeros(3, 6); % measurement matrix - to be determined

%% true trajectory

% sensor trajectory
p_sensor = zeros(3,N+1);
for k = 1:1:N+1
    p_sensor(1,k) = 20 + 20*cos(2*pi/30 * (k-1));
    p_sensor(2,k) = 20 + 20*sin(2*pi/30 * (k-1));
    p_sensor(3,k) = 50;
end

% true target trajectory
x_true = zeros(6,N+1); 
x_true(:,1) = [10; -10; 0; -1; -2; 0]; % initial true state
for k = 2:1:N+1
    x_true(:,k) = F*x_true(:,k-1) + B*u;
end

%% extended 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) = x_true(:,1) + normrnd(0, sig_init);
    P = [eye(3)*sig_init(1)^2, zeros(3); zeros(3), eye(3)*sig_init(4)^2];
    P_diag(:,1) = diag(P);
    for k = 2:1:N+1
        
        %%% Prediction
      
        % predicted state estimate
        x_est(:,k) = F*x_est(:,k-1) + B*u;
        
        % predicted error covariance
        P = F*P*F' + Q;
        
        %%% Update
        % obtain measurement
        p = x_true(1:3,k) - p_sensor(:,k); % true relative position
        z_true = [atan2(p(1), p(2));
                  atan2(p(3), sqrt(p(1)^2 + p(2)^2));
                  norm(p)]; % true measurement
              
        z = z_true + normrnd(0, sig_mea_true); % erroneous measurement
        
        % predicted meausrement
        pp = x_est(1:3,k) - p_sensor(:,k); % predicted relative position
        z_p = [atan2(pp(1), pp(2));
               atan2(pp(3), sqrt(pp(1)^2 + pp(2)^2));
               norm(pp)]; % predicted measurement
        
        % measurement residual
        y = z - z_p;
        
        % measurement matrix
        H = [pp(2)/(pp(1)^2+pp(2)^2), -pp(1)/(pp(1)^2+pp(2)^2), 0, zeros(1,3);
            -pp(1)*pp(3)/(pp'*pp)/norm(pp(1:2)), -pp(2)*pp(3)/(pp'*pp)/norm(pp(1:2)), 1/norm(pp(1:2)), zeros(1,3);
            pp(1)/norm(pp), pp(2)/norm(pp), pp(3)/norm(pp), zeros(1,3)];
                
        % Kalman gain
        K = P*H'/(R+H*P*H');
        
        % updated state estimate
        x_est(:,k) = x_est(:,k) + K*y;
        
        % updated error covariance

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

🌈4 Matlab代码、数据