资源简介
GPS/INS位置组合输出校正Matlab仿真
文件说明:
s_GPS_INS_position_sp_demo.m 组合主文件
kalman_GPS_INS_position_sp_NFb.m 卡尔曼滤波程序
ode500.mat 飞机飞行轨迹与INS输出数据
将三个文件放到同一个文件夹中,运行主文件即可。
希望对此方向的初学者有所帮助。
另附仿真结果。说明:仿真结果用的数据文件时间为3h,附的数据文件时间为500s。

代码片段和文件信息
%GPS/INS无反馈位置组合 卡尔曼滤波器
%%%%%%%%%%%%%%%%%%
%edit by horsejun
%%%%%%%%%%%%%%%%%%
%每秒更新一次速度位置误差
%连续状态系统方程
%dx = F*x + G*w
%z = H*x + v
%离散状态系统方程
%x(k+1) = A*x(k) + B*w(k)
%z(k+1) = C*x(k+1) + v(k+1)
function [E_attitude E_velocity E_position PP] = kalman_GPS_INS_position_sp_NFb(Dp v p quat Fn Q R Tg Ta tao)
%输入
%Dp 量测位置误差, 作为滤波器输入
%Dv 量测速度误差, 作为滤波器输入,
%p ins输出位置,作为滤波器系统参数
%v ins输出速度,作为滤波器系统参数
%fn ins输出导航系下比力,作为滤波器参数
%quat ins输出四元数,作为滤波器参数
%Q 系统噪声方差
%R 测量噪声方差
%Ta 加表误差漂移相关时间
%Tg 陀螺仪误差漂移相关时间
%tao 迭代步长
%%%%%%%输入向量均为行向量%%%%%%%%%%%%%
%输出
%E_position 位置预测值
%E-velocity 速度预测值
%各参数初始化
Re = 6378245; %地球长半径
e = 1/298.257; %地球扁率
wie = 7.292e-5; %地球自转角速度
% 东北天速度
Ve0 = v(:1);
Vn0 = v(:2);
Vu0 = v(:3);
% 导航位置
L0 = p(:1);
h0 = p(:3);
%卡尔曼滤波参数初始化
PP(1:181:18) = diag([1/(36*57) 1/(36*57) 1/57 0.0001 0.0001 0.0001 0 0 1 0.1/(57*3600) 0.1/(57*3600) 0.1/(57*3600) 0.04/(57*3600) 0.04/(57*3600) 0.04/(57*3600) 1e-4 1e-4 1e-4].^2); %初始误差协方差阵
PP0 = PP;
X = zeros(181); %初始状态
E_attitude = zeros(13);
E_position = zeros(13);
E_velocity = zeros(13);
n = size(Dp1);
for i=1:n-1
%参数赋值
Ve = Ve0(i);
Vn = Vn0(i);
Vu = Vu0(i);
L = L0(i);
h = h0(i);
fe = Fn(i1);
fn = Fn(i2);
fu = Fn(i3);
Rm = Re*(1-2*e+3*e*sin(L)^2);
Rn = Re*(1-e*sin(L)^2);
%由四元数计算姿态阵
q = quat(i:);
Cnb = [1-2*(q(3)^2+q(4)^2) 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)) 1-2*(q(2)^2+q(4)^2) 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 1-2*(q(2)^2+q(3)^2)];
%连续系统状态转换阵 F 的时间更新
F = zeros(1818);
F(12) = wie*sin(L)+Ve*tan(L)/(Rn+h);
F(13) = -(wie*cos(L)+Ve/(Rn+h));
F(15) = -1/(Rm+h);
F(19) = Vn/(Rm+h)^2;
F(21) = -(wie*sin(L)+Ve*tan(L)/(Rn+h));
F(23) = -Vn/(Rm+h);
F(24) = 1/(Rn+h);
F(27) = -wie*sin(L);
F(29) = -Ve/(Rn+h)^2;
F(31) = wie*cos(L)+Ve/(Rn+h);
F(32) = Vn/(Rm+h);
F(34) = tan(L)/(Rn+h);
F(37) = wie*cos(L)+Ve*(sec(L)^2)/(Rn+h);
F(39) = -Ve*tan(L)/(Rn+h)^2;
F(42) = -fu;
F(43) = fn;
F(44) = Vn*tan(L)/(Rm+h)-Vu/(Rm+h);
F(45) = 2*wie*sin(L)+Ve*tan(L)/(Rn+h);
F(46) = -(2*wie*cos(L)+Ve/(Rn+h));
F(47) = 2*wie*cos(L)*Vn+Ve*Vn*sec(L)^2/(Rn+h)+2*wie*sin(L)*Vu;
F(49) = (Ve*Vu-Ve*Vn*tan(L))/(Rn+h)^2;
F(51) = fu;
F(53) = -fe;
F(54) = -2*(wie*sin(L)+Ve*tan(L)/(Rn+h));
F(55) = -Vu/(Rm+h);
F(56) = -Vn/(Rm+h);
F(57) = -(2*wie*cos(L)+Ve*(sec(L)^2)/(Rn+h))*Ve;
F(59) = (Ve^2*tan(L)+Vn*Vu)/(Rn+h)^2;
F(61)
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 78336 2008-05-20 16:48 GPS_INS位置组合Matlab仿真源码\GPS_INS位置组合结果.doc
文件 4650 2008-05-22 17:20 GPS_INS位置组合Matlab仿真源码\kalman_GPS_INS_position_sp_NFb.m
文件 631454 2008-05-20 15:14 GPS_INS位置组合Matlab仿真源码\ode500.mat
文件 4172 2008-05-22 17:22 GPS_INS位置组合Matlab仿真源码\s_GPS_INS_position_sp_demo.m
文件 428 2008-05-22 17:25 GPS_INS位置组合Matlab仿真源码\程序说明.txt
目录 0 2009-02-25 10:02 GPS_INS位置组合Matlab仿真源码
----------- --------- ---------- ----- ----
719040 6
- 上一篇:两轮差速开环控制仿真程序
- 下一篇:Matlab2014_(32and64).txt
相关资源
- matlab_OFDM调制解调(来自剑桥大学)
- Matlab路面裂缝识别69319
- 高灵敏度GPS接收机MATLAB仿真,附捕获
- 基于MATLAB的质点弹道计算与外弹道优
- 阵列天线的matlab仿真
- MATLAB 经典程序源代码大全
- MATLAB小波软阈值去噪代码33473
- 天线阵的波束形成在MATLAB仿真程序及
- 非线性SVM算法-matlab实现
- 《MATLAB 智能算法超级学习手册》-程序
- 组合导航matlab程序
- 读取txt文件内容matlab代码实现
- Matlab实现基于相关的模板匹配程序
- matlab优化工具箱讲解
- 基于MATLAB的快速傅里叶变换
- 光纤传输中的分布傅立叶算法matlab实
- 基于matlab的图像处理源程序
- matlab 椭圆拟合程序
- 算术编码解码matlab源代码
- optical_flow 光流法 matlab 实现程序
- 引导图像滤波器 Matlab实现
- 分形几何中一些经典图形的Matlab画法
- OFDM系统MATLAB仿真代码
- SVM工具箱(matlab中运行)
- 图像小波变换MatLab源代码
- LU分解的MATLAB实现
- 冈萨雷斯数字图像处理matlab版(第三
- 替代数据法的matlab程序
- 用matlab实现的多站定位系统性能仿真
- 通过不同方法进行粗糙集属性约简m
评论
共有 条评论