UKF应用于GPS-IMU组合导航系统的MATLAB代码

精品文档 1欢迎下载 组合导航系统的计算程序代码组合导航系统的计算程序代码 function yy ukf IMUgps function ukf IMUgps UKF在IMU GPS组合导航系统中应用 以IMU中的位置 速度 姿态误差角 陀螺漂移常值为状态量 以GPS中的位置 速度为观测量 7 July 2008 clc Initialise state global we RN RM g fl deta wg Tt wt d ww v u W Rbl Ta wa d 0 验证循环次数 地球自转角速度we rad s we 7 292115e 5 g 9 81 地球重力加速度 m s 2 a 6 378137e 6 地球长半轴 e2 0 0066944 地球第一偏心率的平方 姿态角初始值 r p y zitai pi 180 0 2 0282 196 9087 姿态误差角 fai pi 180 1 36 1 36 5 36 100 100 500 r zitai 1 fai 1 p zitai 2 fai 2 y zitai 3 fai 3 当地坐标系 l 相对于载体坐标系 b 的转换矩阵 Rbl 在e n u坐标系下 Rbl cos r cos y sin r sin y sin p sin y cos p cos y sin r sin y sin p cos r cos r sin y sin r cos y sin p cos y cos p sin y sin r cos y sin p cos r 精品文档 2欢迎下载 cos p sin r sin p cos p cos r 由转换矩阵Rbl计算四元数的初始值Q0 q1 q2 q3 q4 QQ 0 5 sqrt 1 Rbl 1 1 Rbl 2 2 Rbl 3 3 0 25 Rbl 3 2 Rbl 2 3 0 5 sqrt 1 Rbl 1 1 Rbl 2 2 Rbl 3 3 0 25 Rbl 1 3 Rbl 3 1 0 5 sqrt 1 Rbl 1 1 Rbl 2 2 Rbl 3 3 0 25 Rbl 2 1 Rbl 1 2 0 5 sqrt 1 Rbl 1 1 Rbl 2 2 Rbl 3 3 IMU系统给出的初始值 速度 ve vn vu 位置 l m h 经度 纬度 高度 姿 态误差角fai e n u vIMU 21 775011 71 6310273 10094 point IMU 0 1470229860 818373653122 826 T 1 UKF滤波的采样周期 s 陀螺常值漂移初始值tuo e n u 单位 s tuo 0 0 0 陀螺一阶相关时间Tt s Tt 60 60 60 加速度计常值漂移初始值jiasu e n u 单位 m s 2 jiasu 0 0 0 加速度计一阶反相关时间Ta s Ta 60 60 60 IMU系统的状态向量X x vIMU point IMU fai tuo jiasu 观测量噪声V的斜方差矩阵 R GPS系统的量测值Z vn ve vd m l h z Rz gps tiqushuju Vk的噪声序列方差为 Rk Rz Rz T 精品文档 3欢迎下载 陀螺常值漂移wt e n u wt pi 180 1 3600 1 3600 1 3600 1 h 陀螺常值漂移误差wtt e n u wtt pi 180 0 08 3600 0 08 3600 0 08 3600 0 08 h 加速度计常值漂移wa e n u wa 2e 6 2e 6 2e 6 200 g 2e 6 m s 2 加速度计常值漂移误差waa e n u waa 2e 6 2e 6 2e 6 4 50 g 0 5e 6 m s 2 姿态误差角噪声wg wg wt 状态向量X的斜方差矩阵 P eye length x eps note for stability P should never be quite zero 速度误差 0 1m s 位置误差 水平 1m 高度 5m u 0 1 0 1 0 1 0 000474429 0 000474429 2 wg wtt waa IMU系统在载体坐标系下的比力值输出值fb fb IMU系统中陀螺输出量 wib 为载体坐标系 b 相对于惯性坐标系 i 的角速度向量 f w IMU tiqushuju IMU系统输出的比力值和角速度 通过GPS观测值计算得到的姿态角 zitaijiao gps xlsread D myfile UKF kalmanfilter MATLAB germany ukf 原始数据 zitiajiao gps xls 循环开始 for 1 n outx outp detajiao zeros 3 1 NN 1000 for i 1 NN outx outx x outp outp diag P 大地子午圈曲率半径 RM 精品文档 4欢迎下载 RM a 1 e2 1 e2 sin x 5 2 2 3 地球卯酉圈的曲率半径 RN RN a sqrt 1 e2 sin x 5 2 当地坐标系下的比力值fl IMU系统在载体坐标系下的比力值输出值fb转换到当地坐标系 l 下fl fb f i fl Rbl fb x 13 x 14 x 15 初始对准 比力值加上加速度计的测量偏差 fl Rbl fb 计算IMU的速度 位置输出减去GPS相应的输出值 deta ve vn l h zd z i deta x 1 2 5 6 zd 1 2 5 6 观测值zz 及观测噪声R zz z i 1 v Rz i 1 zitai v 0 001 0 0266 0 9664 GPS观测值姿态角的误差 zitai v 0 00001 7 0983e 004 0 0006 GPS观测值姿态角的误差 v v zitai v R diag v 2 通过GPS观测值 计算得到roll 0 pitch atan ve vn yaw asin h12 s12 将 姿态误差角加入观测量中进行滤波计算 zz zz detajiao GPS计算得到的姿态角 z zitai zitaijiao gps i 1 IMU系统的力学编排方程和姿态误差方程离散化之后的截断误差 ve x 1 vn x 2 vu x 3 l x 4 m x 5 h x 6 faie x 7 fain x 8 faiu x 9 tuo1 x 10 tuo2 x 11 tuo3 x 12 fl1 fl 1 fl2 fl 2 fl3 fl 3 deta1 deta 1 deta2 deta 2 deta3 deta 3 deta4 deta 4 wg1 wg 1 wg2 wg 2 wg3 wg 3 jiasu1 x 13 jiasu2 x 14 jiasu3 x 15 Ta1 Ta 1 Ta2 Ta 2 Ta3 Ta 3 wa1 wa 1 wt2 wa 2 wt3 wa 3 Q diag u 2 精品文档 5欢迎下载 predict x P predict x P u Q T update x P update x P zz R xx x x xx 1 15 1 u xx 16 30 1 u 1 u 1 x 13 u 2 u 2 x 14 u 3 u 3 x 15 u 7 x 10 u 8 x 11 u 9 x 12 PP P P PP 1 15 1 15 利用四元数Q计算转换矩阵Rbl 首先计算在当地坐标系 l 下的角速度向量wbl 地固坐标系 e 相对于当地坐标系 l 的转换矩阵 Rel Rle Rel sin x 4 cos x 4 0 sin x 5 cos x 4 sin x 5 sin x 4 cos x 5 cos x 5 cos x 4 cos x 5 sin x 4 sin x 5 wie 0 0 we wie 为地球自转角速度向量 omiga12 for k 0 1 wib w i k T x 10 x 11 x 12 初始对准 角速度加上陀螺漂移 wiel 0 we cos x 5 we sin x 5 wiel Rel wie well x 2 RM x 6 x 1 RN x 6 x 1 tan x 5 RN x 6 will wiel well wlb wib Rbl will 精品文档 6欢迎下载 四元数的更新 计算反对称矩阵omiga omiga 0 wlb 3 wlb 2 wlb 1 wlb 3 0 wlb 1 wlb 2 wlb 2 wlb 1 0 wlb 3 wlb 1 wlb 2 wlb 3 0 omiga12 omiga12 omiga end omiga1 omiga12 1 4 1 4 omiga2 omiga12 1 4 5 8 采用四阶龙格 库塔法数值积分解Q K 1 QQ QQ 0 5 T omiga1 omiga2 8 1 T 2 omiga1 2 omiga1 omiga2 omiga2 2 48 1 T 3 omiga1 2 omiga2 omiga1 omiga2 2 384 1 T 4 omiga1 2 omiga2 2 QQ 由Q k 1 可得Rbl k 1 Rbl QQ 1 2 QQ 2 2 QQ 3 2 QQ 4 2 2 QQ 2 QQ 3 QQ 1 QQ 4 2 QQ 2 QQ 4 QQ 1 QQ 3 2 QQ 2 QQ 3 QQ 1 QQ 4 QQ 1 2 QQ 2 2 QQ 3 2 QQ 4 2 2 QQ 3 QQ 4 QQ 1 QQ 2 2 QQ 2 QQ 4 QQ 1 QQ 3 2 QQ 2 QQ 1 QQ 4 QQ 3 QQ 1 2 QQ 2 2 QQ 3 2 QQ 4 2 由Rbl k 1 可得姿态角 翻滚角r 俯仰角p 航向角y k 1 实际导航系 p系 相对于理想导航系 n系 存在误差角fai e n u Cpn为校正矩阵 当方位角为大失准角时 Cpn cos x 9 sin x 9 x 8 cos x 9 x 7 sin x 9 sin x 9 cos x 9 x 8 sin x 9 x 7 cos x 9 x 8 x 7 1 当三个方向的失准角为小角度时 Cpnn 1 x 9 x 8 x 9 1 x 7 x 8 x 7 1 if abs x 9 180 60 pi 10 当姿态误差角 u 即方位角失准角小于 10 时的情况 精品文档 7欢迎下载 Rbl Cpnn Rbl 小失准角 else Rbl Cpn Rbl 大失准角 end zitai0 atan Rbl 3 1 Rbl 3 3 asin Rbl 3 2 atan Rbl 1 2 Rbl 2 2 if Rbl 2 2 0 end if Rbl 2 2 0 zitai0 3 zitai0 3 pi end detajiao zitai0 z zitai detajiao 1 0 detajiao Rbl detajiao if abs detajiao 2 abs x 8 x 8 detajiao 2 zitai0 2 z zitai 2 end if abs detajiao 3 abs x 9 x 9 detajiao 3 zitai0 3 z zitai 3 end zitai zitai zitai0 end 将协方差转换成标准差 outp outp 1 2 将位置误差中的经度和纬度形式转换成当地坐标系 e n u 形式 for k 1 NN 大地子午圈曲率半径 RM 精品文档 8欢迎下载 RM a 1 e2 1 e2 sin outx k 5 2 2 3 地球卯酉圈的曲率半径 RN RN a sqrt 1 e2 sin outx k 5 2 outp k 4 RN outx k 6 cos outx k 5 outp k 4 outp k 5 RM outx k 6 outp k 5 end 绘制高度图 figure plot outx 6 r title UKF计算的高度 germany data figure plot 180 pi outx 8 r title UKF姿态误差角pitch 度 germany data 绘制图 水平运动轨迹 figure plot outx 4 outx 5 b title UKF Level of Movement Germany data xlabel Longitude rad ylabel Latitude rad hold on gpsweizhi xlsread 原始数据 sudu weizhi gps xls plot gpsweizhi 4 gpsweizhi 5 r hold off 绘制图 UKF速度误差 figure subplot 3 1 1 plot outp 1 b title UKF Velocity Error Germany data ylabel Ve m s subplot 3 1 2 plot outp 2 b ylabel Vn m s subplot 3 1 3 plot outp 3 b 精品文档 9欢迎下载 xlabel t s ylabel Vu m s 绘制图 UKF位置误差 figure subplot 3 1 1 plot 100 outp 4 b title UKF Location Error Germany data ylabel x cm subplot 3 1 2 plot 100 outp 5 b ylabel y cm subplot 3 1 3 plot 100 outp 6 b xlabel t s ylabel z cm 绘制图 UKF姿态角误差 figure subplot 3 1 1 plot 3600 180 pi outp 7 b title UKF Attitude Error Germany data ylabel roll second subplot 3 1 2 plot 3600 180 pi outp 8 b ylabel pitch second subplot 3 1 3 plot 3600 180 pi outp 9 b xlabel t s ylabel yaw second xlswrite 计算结果 output x xls outx xlswrite 计算结果 output p xls outp xlswrite 计算结果 output zitai xls zitai 精品文档 10欢迎下载 function x P predict x P u Q T 进行无迹变换 P blkdiag P Q x x u Perform unscented transform x P unscented transform predict model x P T notice how the additional parameter T is passed to the model function y Y unscented transform func dfunc x P varargin 注意其中 dfunc 不知道 P 1 15 1 15 P 1 15 1 15 Q function x P update x P z R x P unscented update observe model observe residual x P z R function x predict model x T global wt Tt Ta wa Rbl 计算预报值 由于UKF使用的是离散时间非线性模型 因此需要对IMU GPS组合系统模型进行离散化处理 这里采用四阶Runge kuta法以数值积分的形式实现 Y k 1 Y k y1 2 y2 2 y3 y4 6 其中 y1 f Y k T y2 f Y k y1 2 T y3 f Y k y2 2 T y4 f Y k y3 T n N size x y for k 1 N xx x k 陀螺漂移常值方程 精品文档 11欢迎下载 xx 10 xx 10 exp 1 Tt 1 wt 1 xx 11 xx 11 exp 1 Tt 2 wt 2 xx 12 xx 12 exp 1 Tt 3 wt 3 加速度计漂移常值方程 xx 13 xx 13 exp 1 Ta 1 wa 1 xx 14 xx 14 exp 1 Ta 2 wa 2 xx 15 xx 15 exp 1 Ta 3 wa 3 x1 detaf xx T T为采样周期 x2 detaf xx x1 2 T x3 detaf xx x2 2 T x4 detaf xx x3 T xx xx x1 2 x2 2 x3 x4 6 xx 16 30 1 zeros 15 1 y y xx 由K状态到K 1状态的预估计 end x y function dta detaf x T global we RN RM fl g deta wg Rbl wt Tt IMU系统的力学编排方程 zhongli 9 7803267714 1 0 00527094 sin x 5 2 0 0000232718 sin x 5 4 0 3086 10 5 x 6 dve 2 we sin x 5 x 1 tan x 5 RN x 6 x 2 2 we cos x 5 x 1 RN x 6 x 3 fl 1 fl 2 x 9 fl 3 x 8 Rbl 1 1 Rbl 1 2 Rbl 1 3 x 13 x 14 x 15 dvn 2 we sin x 5 x 1 tan x 5 RN x 6 x 1 x 2 x 3 RM x 6 fl 2 fl 1 x 9 fl 3 x 7 Rbl 2 1 Rbl 2 2 Rbl 2 3 x 13 x 14 x 15 dvu 2 we cos x 5 x 1 RN x 6 x 1 x 2 2 RM x 6 zhongli fl 3 fl 2 x 7 fl 1 x 8 Rbl 3 1 Rbl 3 2 Rbl 3 3 x 13 x 14 x 15 dl x 1 cos x 5 RN x 6 dm x 2 RM x 6 dh x 3 精品文档 12欢迎下载 IMU系统的姿态误差方程 dfaie we sin x 5 x 1 tan x 5 RN x 6 x 8 we cos x 5 x 1 RN x 6 sin x 9 x 2 1 cos x 9 RM x 6 deta 2 RM x 6 x 2 deta 4 RM x 6 2 Rbl 1 1 Rbl 1 2 Rbl 1 3 x 10 x 11 x 12 dfain we sin x 5 x 1 tan x 5 RN x 6 x 7 x 2 sin x 9 RM x 6 we cos x 5 x 1 RN x 6 1 cos x 9 deta 1 RN x 6 x 1 deta 4 RN x 6 2 we sin x 5 deta 3 Rbl 2 1 Rbl 2 2 Rbl 2 3 x 10 x 11 x 12 dfaiu we cos x 5 x 1 RN x 6 x 8 sin x 9 x 7 cos x 9 x 2 x 8 cos x 9 x 7 sin x 9 RM x 6 deta 1 tan x 5 RN x 6 x 1 deta 4 tan x 5 RM x 6 2 we cos x 5 x 1 sec x 5 2 RN x 6 deta 3 Rbl 3 1 Rbl 3 2 Rbl 3 3 x 10 x 11 x 12 dfaie we sin x 5 x 1 tan x 5 RN x 6 x 8 we cos x 5 x 1 RN x 6 x 9 deta 2 RM x 6 x 2 deta 4 RM x 6 2 x 10 dfain we sin x 5 x 1 tan x 5 RN x 6 x 7 x 2 x 9 RM x 6 deta 1 RN x 6 x 1 deta 4 RN x 6 2 we sin x 5 deta 3 x 11 dfaiu we cos x 5 x 1 RN x 6 x 7 x 2 x 8 RM x 6 deta 1 tan x 5 RN x 6 x 1 deta 4 tan x 5 RM x 6 2 we cos x 5 x 1 sec x 5 2 RN x 6 deta 3 x 12 dta T dve dvn dvu dl dm dh dfaie dfain dfaiu 0 0 0 0 0 0 T为采样周期 dta dta zeros 15 1 function z observe model x 观测方程 z Hx v 其中H为单位矩阵 global v m M size x h eye 9 zeros 9 6 zeros 9 15 精品文档 13欢迎下载 z h x repvec v M function dz observe residual z1 z2 Given two observation values compute their normalised residual Notice once again that the model is vectorised in terms of z1 and z2 dz z1 z2 function x repvec x N x x ones 1 N UKFUKF的程序代码的程序代码 function y Y unscented transform func dfunc x P varargin function y Y unscented transform func dfunc x P Algorithm implemented as described in A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators 3 APIRL 2008 INPUTS func function handle myfunc or string myfunc for non linear transform dfunc function handle or string for residual between two transformed values e mydfunc y1 y2 x P initial mean and covariance optional arguments such that func has form y myfunc x OUTPUTS y Y transformed mean and covariance NOTES 1 The unscented filter has two primary advantages over the EKF i it produces a more accurate 精品文档 14欢迎下载 estimate of the first and second moments of a random vector transformed through a non linear function and ii it does not require analytical Jacobians 2 The function func is the non linear function itself and transforms x to y This function may be passed any number of additional parameters eg y myfunc x p1 p2 p3 3 The function dfunc is required to deal with discontinuous functions Some non linear functions are discontinuous but their residuals are not equal to the discontinuity A classic example is a normalised polar measurement y1 myfunc x1 p1 p2 p3 lets say y1 pi y2 myfunc x2 p1 p2 p3 lets say y2 pi dy y1 y2 dy 2 pi this is wrong must be within pi dy mydfunc y1 y2 dy 0 this is correct Thus mydfunc is a function that computes the true residual of y1 y2 If the function myfunc is not discontinuous or has a trivial residual just pass to parameter dfunc 4 The functions func and dfunc must be vectorised That is they must be able to accept a set of states as input and return a corresponding set of results So for func the state x will not be a single column vector but a matrix of N column vectors Similarly for dfunc the parameters y1 and y2 will each be matrices of N column vectors EXAMPLE USE y Y unscented transform myfunc mydfunc x P p1 p2 p3 精品文档 15欢迎下载 a B unscented transform continuous model x P Tim Bailey 2003 modified 2004 2005 Set up some values D length x state dimension N D 2 1 number of samples scale 3 want scale D kappa 3 kappa scale D Create samples Ps chol P sqrt scale ss x repvec x D Ps repvec x D Ps Transform samples according to function func if isempty dfunc dfunc default dfunc end ys feval func ss varargin compute possibly discontinuous transform base repvec ys 1 N set first transformed sample as the base delta feval dfunc base ys compute correct residual ys base delta offset ys from base according to correct residual Calculate predicted observation mean idx 2 N y 2 kappa ys 1 sum ys idx 2 2 scale Calculate new unscented covariance dy ys repvec y N Y 2 kappa dy 1 dy 1 dy idx dy idx 2 scale Note if x is a matrix of column vectors then x x produces the sum of outer products 精品文档 16欢迎下载 function e default dfunc y1 y2 e y1 y2 function x repvec x N x x ones 1 N function x P unscented update zfunc dzfunc x P z R varargin x P unscented update zfunc dzfunc x P z R 3 APIRL 2008 INPUTS zfunc function handle myfunc or string myfunc for observe model dzfunc function handle or string for observation residual v mydfunc z z predict x P predict mean and covariance z R observation and covariance observation noise is assumed additive optional arguments such that zfunc has the form z myfunc x OUTPUTS x P updated mean and covariance NOTES 1 This function uses the unscented transform to compute a Kalman update 2 The function zfunc is the non linear observation function This function may be passed any number of additional parameters eg z my observe model x p1 p2 精品文档 17欢迎下载 3 The residual function dzfunc is required to deal with discontinuous functions Some non linear functions are discontinuous but their residuals are not equal to the discontinuity A classic example is a normalised angle measurement model z1 angle observe model x1 p1 p2 p3 lets say z1 pi z2 angle observe model x2 p1 p2 p3 lets say z2 pi dz z1 z2 dz 2 pi this is wrong must be within pi dz residual model z1 z2 dz 0 this is correct Thus residual model is a function that computes the true residual of z1 z2 If the function zfunc is not discontinuous or has a trivial residual just pass to parameter dzfunc 4 The functions zfunc and dzfunc must be vectorised That is they must be able to accept a set of states as input and return a corresponding set of results So for zfunc the state x will not be a single column vector but a matrix of N column vectors Similarly for dzfunc the parameters z and z predict will be equal sized matrices of N column vectors EXAMPLE USE x P unscented update angle obs