资源简介
程序中完整的给出了matlab组合导航仿真过程
代码片段和文件信息
%GPS/INS无反馈位置组合 卡尔曼滤波器
%每秒更新一次速度位置误差
%连续状态系统方程
%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) = -fn;
F(62) = fe;
F(64) = 2
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 78336 2008-05-20 16:48 GPS_INS位置组合程序——好\GPS_INS位置组合结果.doc
文件 4593 2008-09-19 21:41 GPS_INS位置组合程序——好\kalman_GPS_INS_position_sp_NFb.asv
文件 4592 2008-11-05 20:28 GPS_INS位置组合程序——好\kalman_GPS_INS_position_sp_NFb.m
文件 7256 2006-12-20 15:43 GPS_INS位置组合程序——好\KF_SINS.m
文件 631454 2008-05-20 15:14 GPS_INS位置组合程序——好\ode500.mat
文件 21292 2008-11-26 20:27 GPS_INS位置组合程序——好\shixiong.m
文件 4104 2008-08-28 14:28 GPS_INS位置组合程序——好\s_GPS_INS_position_sp_demo.asv
文件 4017 2009-03-02 11:03 GPS_INS位置组合程序——好\s_GPS_INS_position_sp_demo.m
文件 162 2008-09-13 10:00 GPS_INS位置组合程序——好\~$S_INS位置组合结果.doc
文件 351 2009-03-02 10:46 GPS_INS位置组合程序——好\程序说明.txt
目录 0 2013-07-31 08:42 GPS_INS位置组合程序——好
----------- --------- ---------- ----- ----
756157 11
- 上一篇:mcgs项目例程
- 下一篇:CT模型机动目标圆周运动跟踪
相关资源
- matlab编写的GPS和惯导的组合程序
- 多传感器组合导航系统
- INS/GPS 惯性卫星组合导航。输入惯导
- kalman_filter2 北航研究生课程卡尔曼滤
- BP神经网络辅助组合导航卡尔曼滤波
- GNSS与惯性及多传感器组合导航系统原
- GPS_INS_integratede_PPT 加拿大卡尔加里大
- Loosecombination
- UKF-GPS-IMU-MATLAB
- GPS-inertial-navigation- GPS惯性导航组合(
- auv-navigation AUV惯性导航系统matlab仿真
- zuhedaohang 捷联惯导和GPS组合导航程序
- GPS_SINS-integrated-navigation 高动态惯导
- GPS_INS SINS和GPS组合导航
评论
共有 条评论