clc;clear?all;close?all;
T=1;??%采樣周期
N=100/T;??%總的采樣次數(shù)
r=1.01;var_u=1;var_w=3;???%幾個(gè)參數(shù)初始化賦值
A=[r000;0r00;T*（1+r）/2010;0T*（1+r）/201];
B=[10T/20;010T/2]‘;
Q=var_u*eye（2）;
H=[0010;0001];
p11=（T^2*var_u+8*r^2*var_w）/（T^2*（1+r）^2）;
p13=2*r*var_w/（T*（1+r））;
M22=[p110p130;0p110p13;p130var_w0;0p130var_w];

xd=zeros（2N）;
S=zeros（4N）;??%信號(hào)矢量vxvyxy
S（:1）=[1111];?%x四維，初始絕對位置（11）水平速度2，垂直速度2???%（:1）取第一列
Z=zeros（2N）;??
Z（:1）=[S（31）S（41）];??%z只觀測到位置，沒有速度，zx觀測值
Z1=zeros（2N）;???????????%z真實(shí)相對觀測值
Z1（:1）=Z（:1）;??%z只觀測到位置，沒有速度，zx觀測值
Skf=zeros（4N）;??%卡爾曼濾波結(jié)果
Skf（:1）=S（:1）;?
%%%運(yùn)動(dòng)模型，狀態(tài)模型s（n）=As（n-1）+Bn和觀測到的Z
for?i?=?2:N
????S（:i）=A*S（:i-1）+B*var_u*randn（21）;
????Z（:i）=H*S（:i）+var_w*randn（21）;
????xd（:i）=xd（:i-1）+[1;0];???????%沿著x軸勻速????
????Z1（:i）=Z（:i）-xd（:i）;????%觀測真實(shí)相對值

end
zx1=Z（11）;zx2=Z（12）;zy1=Z（21）;zy2=Z（22）;
db=2*r/（T*（1+r））;
Skf（:2）=[db*（zx2-zx1）db*（zy2-zy1）zx2zy2];%s[2|2]
%%%濾波的
for?i=3:N??
????S_pre=A*Skf（:i-1）;???????????????????????????%s[k|k-1]=As[k-1|k-1]
????M_pre=A*M22*A‘+var_w*（B*B‘）;??????????????????%M[k|k-1]=AM[k-1|k-1]A‘+var_w*BB‘
????K=M_pre*H‘/（H*M_pre*H‘+var_w*eye（2））;?????????%K[k]=M[k|k-1]H‘（HM[k|k-1]H‘+var_w*I）-1
????Skf（:i）=S_pre+K*（Z（:i）-H*S_pre）;??
????M22=（eye（4）-K*H）*M_pre;??????????????????????%M[k|k]=（I-KH）*M[k|k-1]
end??

%%%繪圖
figure;
hold?on;
plot（S（3:）S（4:）‘-k‘）;??%位置軌跡
plot（Z（1:）Z（2:）‘-b.‘）;??%觀測軌跡
%?plot（Z1（1:）Z1（2:）‘-m*‘）;?%真實(shí)位置
plot（Skf（3:）Skf（4:）‘-r+‘）;??%Kalman估計(jì)軌跡
hold?off;legend（‘真實(shí)軌跡‘‘觀測軌跡‘‘濾波軌跡‘）??
%%%誤差計(jì)算模塊
for?i=1:N??
????Err_Observation（i）=RMS（S（:i）Z（:i））;??
????Err_KalmanFilter（i）=RMS（Skf（:i）Z（:i））;??
end?
figure;??
hold?on;?box?on;??
plot（Err_Observation‘-ko‘‘MarkerFace‘‘g‘）??
plot（Err_KalmanFilter‘-ks‘‘MarkerFace‘‘r‘）??
legend（‘濾波前誤差‘‘濾波后誤差‘）?

%%%第二問引入傳感器誤差引入改變之后的矩陣
%?var_d=2;??????%傳感器方差
%?var_n=var_d+var_w;%M[2|2]中出現(xiàn)的
%?p11_n=（T^2*var_u+8*r^2*var_n）/（T^2*（1+r）^2）;
%?p13_n=2*r*var_n/（T*（1+r））;
%?M22_n=[p11_n0p13_n0;0p11_n0p13_n;p13_n0var_n0;0p13_n0var_n];
%?Skf_n=zeros（4N）;??%卡爾曼濾波結(jié)果
%?Skf_n（:1）=S（:1）;?

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????2372??2020-04-12?19:33??卡爾曼濾波\kal_test.m

?????文件???????2376??2020-04-12?19:36??卡爾曼濾波\kal_test2.m

?????文件????????174??2020-04-12?10:52??卡爾曼濾波\RMS.m

?????文件?????149886??2020-04-14?23:16??卡爾曼濾波\題目描述1.png

?????文件??????41930??2020-04-14?23:17??卡爾曼濾波\題目描述2.png

?????目錄??????????0??2020-04-14?23:18??卡爾曼濾波

-----------?---------??----------?-----??----

???????????????196738????????????????????6