最优估计大作业：基于kalman滤波的组合导航

给定磁罗盘、GPS、里程计等信息，进行组合导航数据融合。采用kalman滤波方法。 包含报告和程序。

% read original data
X = load（‘z+n.txt‘）;
posOfEast = X（: 1）;
posOfNorth = X（: 2）;
GPSHeading = heading_preprocess（X（: 3））;
% GPSHeading = X（: 3）;
GPSSpeed = X（: 4）;
GyroAngularRate = X（: 5）;
Odometer = X（: 6）;

% add gaussian white noise （mean=0 variance=8） 
posOfEastNoisy = posOfEast + randn（length（posOfEast） 1）*8;
posOfNorthNoisy = posOfNorth + randn（length（posOfEast） 1）*8;

% draw curve with and without noise to compare
figure（1）
x = 1:length（posOfEast）;
subplot（211）;
plot（x posOfEast-690000 ‘g‘）;
hold on;
plot（x posOfEastNoisy-690000 ‘r‘）;
legend（‘original data‘ ‘noisy data‘）;
ylabel（‘posOfEast‘）;
subplot（212）;
plot（x posOfNorth-4850000 ‘g‘）;
hold on;
plot（x posOfNorthNoisy-4850000 ‘r‘）;
legend（‘original data‘ ‘noisy data‘）;
ylabel（‘posOfNorth‘）;

% generate observed data
Z = [posOfNorthNoisy‘;posOfEastNoisy‘;GPSHeading‘;GPSSpeed‘;...
    GyroAngularRate‘];

% draw curve of original data and observed data to compare similar to
% figure 1
figure（2）;
x = 1:length（posOfEast）;
subplot（511）;
plot（x posOfNorth-4850000 ‘g‘）;
hold on;
plot（x Z（1:）-4850000 ‘r‘）;
ylabel（‘posOfNorth‘）;
subplot（512）;
plot（x posOfEast-690000 ‘g‘）;
hold on;
plot（x Z（2:）-690000 ‘r‘）;
ylabel（‘posOfEast‘）;
subplot（513）;
plot（x GPSHeading ‘g‘）;
hold on;
plot（x Z（3:） ‘r‘）;
ylabel（‘GPSHeading‘）;
subplot（514）;
plot（x GPSSpeed ‘g‘）;
hold on;
plot（x Z（4:） ‘r‘）;
ylabel（‘GPSSpeed‘）;
subplot（515）;
plot（x GyroAngularRate ‘g‘）;
hold on;
plot（x Z（5:） ‘r‘）;
ylabel（‘GyroAngularRate‘）;

% set Q and R matrix
% Q = eye（7） * 1;
% R = eye（5） * 10;
Q = diag（[0.2 0.2 0.2 2 0.2 0.2 0.2]）;
R = diag（[8 8 0.01 0.01 0.01]）;

% kalman filter using ‘transfer matrix generator‘
%   X00:[Ny Ex psi v psi_dot B S]
Xk1k1_hat = [posOfNorth（1）;posOfEast（1）;GPSHeading（1）;GPSSpeed（1）;...
    GyroAngularRate（1）;0.1;0.1];
%   P00:7*7 matrix
Pk1k1 = 0.1*rand（7）;

%   kalman main loop
%       X_hat is used to store estimation
X_hat = zeros（7 length（posOfEast））;
X_hat（:1） = Xk1k1_hat;
for idx = 2:length（posOfEast）
    % one step: cal from down to top
    %   cal P（k+1|k）:using X（k|k）
    Pk1k = phi（Xk1k1_hat（3）） * Pk1k1 * phi（Xk1k1_hat（3））‘ + ...
        gamma（） * Q * gamma（）‘;
    %   cal K（k+1）:using Z（k+1） and P（k+1|k） which is P（k+1|k+1） of last
    %   step
    Kk1 = Pk1k * h（Odometer（idx））‘ * （（h（Odometer（idx）） * Pk1k * ...
        h（Odometer（idx））‘ + R） ^ -1）;
    %   cal X（k+1|k）:using X（k|k）
    Xk1k_hat = phi（Xk1k1_hat（3）） * Xk1k1_hat;
    %   cal X（k+1|k+1）:using X（k+1|k） and Z（k+1）
    Xk1k1_hat = Xk1k_hat + Kk1 * （Z（:idx） - h（Odometer（idx）） * Xk1k_hat）;
    X_hat（:idx） = Xk1k1_hat;
    %   cal P（k+1|k+1）
    Pk1k1 = Pk1k - Kk1 * h（Odometer（idx）） * Pk1k;
end

% draw curve of observed data and filtered data to compare
figure（3）;
subplot（211）;
x = 1:length（posOfNorthNoisy）;
plot（x posOfNorthNoisy ‘y‘）;
hold on;
plot（x X_hat（1 :） ‘r‘）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    .CA....      5239  2018-05-10 00:01  组合导航作业\main.m

    .CA....      4636  2018-05-09 23:22  组合导航作业\main_more_analysis.m

    .CA....     53776  2008-11-12 16:10  组合导航作业\z+n.txt

    .CA....   1086099  2019-03-17 17:21  组合导航作业\第二章_组合导航作业20180510.docx

    .C.D...         0  2019-03-17 17:24  组合导航作业

----------- ---------  ---------- -----  ----

              1149750                    5