clear;
close?all
clc;
N=1458;?
TrueVal=（ones（1N））‘;???
Val=（ones（1N））‘;?????
fid?=?fopen（‘yout.txt‘‘r‘）?;?%fopen打開文獻
?TrueVal?=?fscanf（fid‘%f‘）;?%fscand格式化輸入空格或者換行時，結束???
?TrueVal=TrueVal‘;
fclose（fid）;??%把緩沖區內最后剩余的數據輸出到磁盤文件中，并釋放文件指針和有關的緩沖區。????
ProcNConvQ?=?0.01;%過程噪聲
Wq=sqrt（ProcNConvQ）*randn（1N）;?%返回一個1*n的矩陣???
MeasNConvR?=0.3;?%測量噪聲???????
Wr?=?sqrt（MeasNConvR）*randn（1N）;?????
MeasureValue_Array?=?TrueVal+Wr;?%
ErrCov_i_prev?=?1;
XState_i_prev?=?4;
for?i=1:N
?MeasureValue_i?=?MeasureValue_Array（i）;
?XAhead_i?=??XState_i_prev;????????????%?X（k|k-1）=X（k-1|k-1）公式3.3.46
?ErrCovAhead_i?=??ErrCov_i_prev??+?ProcNConvQ;????????%?P（k|k-1）=P（k-1|k-1）?+?ProcNConvQ??1e-5?公式3.3.49
?KalGain_i?=?ErrCovAhead_i??/?（ErrCovAhead_i?+?MeasNConvR）;????%?Kg（k）=?P（k|k-1）?/?（P（k|k-1）?+?MeasNConvR）公式3.3.48
?XState_i_current?=?XAhead_i+KalGain_i?*?（MeasureValue_i?-??XAhead_i）;?%?X（k|k）=?X（k|k-1）+Kg（k）?（Z（k）-X（k|k-1））公式3.3.45
?ErrCov_i_current?=?（1-KalGain_i）?*?ErrCovAhead_i;??????%?P（k|k）=（1-Kg（k））P（k|k-1）公式3.3