最近一段时间在尝试用FPGA设计卡尔曼滤波器,设计过程比较复杂,在此过程中卡尔曼滤波器的基本原理,matlab仿真与C语言实现,及其在导航算法中的应用困扰了我很长一段时间。在和师兄交流和参阅些许文献之后,终于小有收获。下面是把卡尔曼滤波器在INS-GPS组合导航中应用仿真的matlab源代码与大家分享,希望对初学者有一定的借鉴价值。
在下面的仿真的代码中,理想的观测量不是真实数据,而是自生成的正弦波数据,在真实的应用场景中,应该是一系列的参考数据。
运行结果:
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- % 卡尔曼滤波器在INS-GPS组合导航中应用仿真
- % Author : lylogn
- % Email : lylogn@gmail.com
- % Company: BUAA-Dep3
- % Time : 2013.01.06
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- % 参考文献:
- % [1]. 邓正隆. 惯导技术, 哈尔滨工业大学出版社.2006.
- clear all;
- %% 惯性-GPS组合导航模型参数初始化
- we = 360/24/60/60*pi/180; %地球自转角速度,弧度/s
- psi = 10*pi/180; %psi角度 / 弧度
- Tge = 0.12;
- Tgn = 0.10;
- Tgz = 0.10; %这三个参数的含义详见参考文献
- sigma_ge=1;
- sigma_gn=1;
- sigma_gz=1;
- %% 连续空间系统状态方程
- % X_dot(t) = A(t)*X(t) + B(t)*W(t)
- A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0;
- -we*sin(psi) 0 0 0 1 0 0 1 0;
- we*cos(psi) 0 0 0 0 1 0 0 1;
- 0 0 0 -1/Tge 0 0 0 0 0;
- 0 0 0 0 -1/Tgn 0 0 0 0;
- 0 0 0 0 0 -1/Tgz 0 0 0;
- 0 0 0 0 0 0 0 0 0;
- 0 0 0 0 0 0 0 0 0;
- 0 0 0 0 0 0 0 0 0;]; %状态转移矩阵
- B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;
- 0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;
- 0 0 0 0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%输入控制矩阵
- %% 转化为离散时间系统状态方程
- % X(k+1) = F*X(k) + G*W(k)
- T = 0.1;
- [F,G]=c2d(A,B,T);
- H=[1 0 0 0 0 0 0 0 0;
- 0 -sec(psi) 0 0 0 0 0 0 0;];%观测矩阵
- %% 卡尔曼滤波器参数初始化
- t=0:T:50-T;
- length=size(t,2);
- y=zeros(2,length);
- Q=0.5^2*eye(3); %系统噪声协方差
- R=0.25^2*eye(2); %测量噪声协方差
- y(1,:)=2*sin(pi*t*0.5);
- y(2,:)=2*cos(pi*t*0.5);
- Z=y+sqrt(R)*randn(2,length); %生成的含有噪声的假定观测值,2维
- X=zeros(9,length); %状态估计值,9维
- X(:,1)=[0,0,0,0,0,0,0,0,0]'; %状态估计初始值设定
- P=eye(9); %状态估计协方差
- %% 卡尔曼滤波算法迭代过程
- for n=2:length
- X(:,n)=F*X(:,n-1);
- P=F*P*F'+ G*Q*G';
- Kg=P*H'/(H*P*H'+R);
- X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n));
- P=(eye(9,9)-Kg*H)*P;
- end
- %% 绘图代码
- figure(1)
- plot(y(1,:))
- hold on;
- plot(y(2,:))
- hold off;
- title('理想的观测量');
- figure(2)
- plot(Z(1,:))
- hold on;
- plot(Z(2,:))
- hold off;
- title('带有噪声的观测量');
- figure(3)
- plot(X(1,:))
- hold on;
- plot(X(2,:))
- hold off;
- title('滤波后的观测量');
复制代码 |