卡尔曼滤波matlab仿真

一、温度测量

[plain]  view plain  copy

1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
2. % 程序说明：Kalman滤波用于温度测量的实例(一维）      
3. function main    
4. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
5. %初始化参数  
6. N=120;  %采样点个数  
7. A=1;    
8. B=1;    
9. H=1;   
10. Q=0.01;    
11. R=0.25;    
12. W=sqrt(Q)*randn(1,N);    
13. V=sqrt(R)*randn(1,N);    
14. CON=25;  %温度真实值  
15. %分配空间  
16. X=zeros(1,N);      
17. Z=zeros(1,N);   
18. Xkf=zeros(1,N);    
19. P=zeros(1,N);     
20. X(1)=25.1;    
21. P(1)=0.01;    
22. Z(1)=24.9;    
23. Xkf(1)=Z(1);    
24. I=eye(1);     
25. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   
26. %kalman过程  
27. for k=2:N    
28.     X(k)=A*X(k-1)+B*W(k);     
29.     Z(k)=H*X(k)+V(k);    
30.     X_pre(k)=A*Xkf(k-1);               
31.     P_pre(k)=A*P(k-1)*A'+Q;            
32.     Kg=P_pre(k)/(H*P_pre(k)*H'+R);               
33.     Xkf(k)=X_pre(k)+Kg*(Z(k)-H*X_pre(k));             
34.     P(k)=(I-Kg*H)*P_pre(k);    
35. end    
36. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
37. %误差过程  
38. Err_Messure=zeros(1,N);    
39. Err_Kalman=zeros(1,N);    
40. for k=1:N    
41.     Err_Messure(k)=Z(k)-X(k);    
42.     Err_Kalman(k)=Xkf(k)-X(k);    
43. end    
44. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
45. t=1:N;    
46. figure;    
47. plot(t,CON*ones(1,N),'-b',t,X,'-r',t,Z,'g+',t,Xkf,'-k');    
48. legend('期望值','真实值','测量值','kalman估计值');             
49. xlabel('采样时间');    
50. ylabel('温度');    
51. title('Kalman Filter Simulation');    
52. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
53. figure;    
54. plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');    
55. legend('测量误差','kalman误差');             
56. xlabel('采样时间');    
57. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%empt  

二、自由落体追踪（二维x,v）

[plain]  view plain  copy

1. %kalman滤波用于自由落体运动目标追踪（二维，x,v)  
2. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
3. %初始化参数  
4. N=1000;  
5. Q=[0,0;0,0];  
6. R=0.5;  
7. w=sqrt(Q)*randn(2,N);  
8. v=sqrt(R)*randn(1,N);  
9. A=[1,1;0,1];%状态转移矩阵  
10. B=[0.5;1];%控制矩阵  
11. U=-10;%控制变量  
12. H=[1,0];%观测矩阵  
13. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
14. %分配空间，x,p0,z,xkf  
15. x=zeros(2,N);%物体真实状态  
16. x(:,1)=[95;1];%初始位移和速度  
17. p0=[10,0;0,1];%初始误差  
18. z=zeros(1,N);%观测值  
19. z(1)=H*x(:,1);%观测真实值,第一列的列向量，  
20. xkf=zeros(2,N);%kalman估计状态  
21. xkf(:,1)=x(:,1);%kalman估计状态初始化,第一列的列向量，  
22. err_p=zeros(N,2);  
23. err_p(1,1)=p0(1,1);  
24. err_p(1,2)=p0(2,2);  
25. I=eye(2);%2*2单位矩阵表明二维系统  
26. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
27. %kalman迭代过程  
28. for k=2:N;  
29.     x(:,k)=A*x(:,k-1)+B*U+w(k);%模型  
30.     z(k)=H*x(:,k)+v(k);%模型  
31.     x_pre=A*xkf(:,k-1)+B*U;%①  
32.     p_pre=A*p0*A'+Q;%②  
33.     kg=p_pre*H'/(H*p_pre*H'+R);%③  
34.     xkf(:,k)=x_pre+kg*(z(k)-H*x_pre);%④  
35.     p0=(I-kg*H)*p_pre;%⑤  
36.     err_p(k,1)=p0(1,1);%位移误差均方值  
37.     err_p(k,2)=p0(2,2);%速度误差均方值  
38. end  
39. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
40. %误差计算  
41. messure_err_x=zeros(1,N);  
42. kalman_err_x=zeros(1,N);  
43. kalman_err_v=zeros(1,N);  
44. for k=1:N  
45.     messure_err_x(k)=z(k)-x(1,k);%位移的测量误差  
46.     kalman_err_x(k)=xkf(1,k)-x(1,k);%kalman估计位移与真实位移之间偏差  
47.     kalman_err_v(k)=xkf(2,k)-x(2,k);%kalman估计速度与真实速度之间偏差  
48. end  
49. %噪声图  
50. figure;  
51. plot(w);xlabel('采样时间');ylabel('过程噪声');  
52. figure;  
53. plot(v);xlabel('采样时间');ylabel('测量噪声');  
54. %位置误差  
55. figure;  
56. hold on,box on;  
57. plot(messure_err_x,'-r.');  
58. plot(kalman_err_x,'-g.');  
59. legend('测量位置','kalman估计位置');  
60. xlabel('采样时间');ylabel('位置偏差');  
61. %kalman速度偏差  
62. figure;  
63. plot(kalman_err_v);  
64. xlabel('采样时间');ylabel('速度偏差');  
65. %均方值  
66. figure;  
67. plot(err_p(:,1));  
68. xlabel('采样时间');ylabel('位移误差均方值');  
69. figure;  
70. plot(err_p(:,1));  
71. xlabel('采样时间');ylabel('速度误差均方值');  

三、GPS导航定位

[plain]  view plain  copy

1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
2. %  Kalman滤波在船舶GPS导航定位系统中的应用(四维，x方向x,v   ,y方向x,v; ）   
3. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
4. function main    
5. T=1;  %采样周期  
6. N=80/T;  %总的采样次数  
7. F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];    
8. H=[1,0,0,0;0,0,1,0];    
9. Q=4e-4  ;  
10. R=100*eye(2);  
11. X=zeros(4,N);    
12. X(:,1)=[100,2,200,20]; %x四维，初始位置（-100,200）水平速度2，垂直速度20   
13. Z=zeros(2,N);    
14. Z(:,1)=[X(1,1),X(3,1)];  %z只观测到位置，没有速度  
15. Xkf=zeros(4,N);    
16. Xkf(:,1)=X(:,1);    
17. P0=eye(4);  %协方差矩阵初始化，4*4单位矩阵  
18. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
19. for t=2:N  %模型  
20.     X(:,t)=F*X(:,t-1)+sqrt(Q)*randn(4,1);%目标真实轨迹  randn(4,1)4*1随机矩阵  
21.     Z(:,t)=H*X(:,t)+sqrt(R)*randn(2,1); %观测轨迹randn(2,1)2*1随机矩阵  
22. end    
23. for i=2:N    
24.     X_pre=F*Xkf(:,i-1);    
25.     P_pre=F*P0*F'+Q;    
26.     K=P_pre*H'/(H*P_pre*H'+R);    
27.     Xkf(:,i)=X_pre+K*(Z(:,i)-H*X_pre);    
28.     P0=(eye(4)-K*H)*P_pre;    
29. end    
30. for i=1:N    
31.     Err_Observation(i)=RMS(X(:,i),Z(:,i));    
32.     Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));    
33. end    
34. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
35. figure;    
36. hold on;box on;    
37. plot(X(1,:),X(3,:),'-k');  %位置轨迹  
38. plot(Z(1,:),Z(2,:),'-b.');  %观测轨迹  
39. plot(Xkf(1,:),Xkf(3,:),'-r+');  %Kalman估计轨迹  
40. legend('真实轨迹','观测轨迹','滤波轨迹')    
41. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
42. figure;    
43. hold on; box on;    
44. plot(Err_Observation,'-ko','MarkerFace','g')    
45. plot(Err_KalmanFilter,'-ks','MarkerFace','r')    
46. legend('滤波前误差','滤波后误差')    
47. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
48. function dist=RMS(X1,X2);    
49. if length(X2)<=2    
50.     dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );    
51. else    
52.     dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );    
53. end    
54. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   

转载于https://blog.csdn.net/try_again_later/article/details/78304691