最近一段时间在尝试用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('滤波后的观测量');

程序运行结果如下三图所示：

http://s12/mw690/70a144584d29b875bafbb&690

http://s8/mw690/70a144584d29b890bd817&690

http://s15/mw690/70a144584d29b8b689f3e&690

注：以上源代码仅供参考，学习交流之用。

参考文献：

[1]. http://en.wikipedia.org/wiki/Kalman_filter

[2]. http://en.wikipedia.org/wiki/Covariance

[3]. 邓正隆. 惯导技术, 哈尔滨工业大学出版社.2006.