1 简介
在捷联惯导系统（SINS）中惯性测量器件（陀螺和加速度计）直接与运载体固联，通过导航计算机采集惯性器件的输出信息并进行数值积分求解运载体的姿态、速度和位置等导航参数，这三组参数的求解过程即所谓的姿态更新算法、速度更新算法和位置更新算法。特别在恶劣的高动态环境下，高精度的 SINS 对惯性器件性能和导航算法精度的要求都非常苛刻，由于高精度惯性器件往往价格昂贵并且进一步提升精度异常困难，所以在影响 SINS 精度的所有误差源中要求因导航算法引起的误差比重必须很小，一般认为应小于 5%。姿态更新算法是 SINS 算法的核心，对整个系统的解算精度影响最为突出，具有重要的研究和应用价值。传统的姿态更新算法有欧拉角法、方向余弦阵法和四元数法等方法，这些方法直接以陀螺采样输出作为输入，使用泰勒级数展开或龙格—库塔等方法求解姿态微分方程，未充分考虑转动的不可交换性误差问题。传统姿态更新算法在理论上可以通过提高采样和更新频率来提高解算精度，但实际陀螺采样频率又受限于传感器的带宽和噪声水平，因此传统算法的精度提升空间相对有限，仅适用于对解算精度要求不太高的场合。基于卡尔曼滤波实现无人机捷联惯导算法与组合导航。
2 部分代码
function [ theta, omega ] = caculate_theta( ang_motion, crt )% 名称：Caculate theta(angule) form the seted angule motion% 功能：给定预设角运动形式，计算current时刻的角度% 注意：目前还没有添加规范化姿态角的功能，因此尽量不要把摇摆幅值设的过大%% Inputs:% ang_motion: 预设角运动，每一行对应着一个形如：% theta = A*sin( w*t + phi) + k的运动，载体角运动由若干个% 正弦运动叠加而成% crt: 当前时间% Output:% theta: 跟预设角运动所对应的姿态 (rad)% omega: 与预设角运动所对应的当前时刻角速度 (rad/s)% Author: Kun Gan, Tongji University% Email : ciaotigre@126.com% Date : 2020/12/1%%% i表示 ang_motion 中的第i行i = 1;% 如果ang_motion中存在第i行while i <= size(ang_motion, 1) if i == 1 theta = 0; % rad omega = 0; % rad/s end % ang_motion.p = [A1(幅值), w1(角速度), phi1(初始相位), k1(中心值) % A2(幅值), w2(角速度), phi2(初始相位), k2(中心值) % ... ] % 注意:为了编程的简单和简洁，ang_motion中的变量单位为rad或rad/s A = ang_motion(i, 1); w = ang_motion(i, 2); phi = ang_motion(i, 3); k = ang_motion(i, 4); % 姿态角 theta = A*sin( w*crt + phi) + k + theta; % wnbnx nx分别是(n n1), (n1 n2), (n1 b) omega = w*A*cos(w*crt + phi) + omega; % i+1, 准备检查ang_motion是否存在下一行 i = i+1; endend
3 仿真结果
4 参考文献
[1]景丽. 基于卡尔曼滤波组合导航算法的计算量与精度分析[D]. 哈尔滨工业大学, 2014.
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。
部分理论引用网络文献，若有侵权联系博主删除。
5 代码下载
