%?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