Psins代码解析之后处理POS轨迹仿真(test_POS_trj.m)&后处理POS融合(test_POS_fusion.m)

旧城等待， 2023-07-23 13:58 47阅读 0赞

代码中涉及到的地球参数、全局变量等参数详见：

[Psins代码解析之全局变量&轨迹仿真（test\_SINS\_trj.m）&惯性解算（test\_SINS.m）][Psins_test_SINS_trj.m_test_SINS.m]

### **一、POS轨迹生成：**test\_POS\_trj.m ###

初始姿态为0、初始速度为0、初始位置为【34.246048; 108.909664; 380】

描述一段完整的运动轨迹：

*  初始化；
 *  静止100秒；
 *  向北方向飞行，加速度5m/s^2，持续20秒；速度最大为100m/s；
 *  以100m/s继续向北，飞行10秒；
 *  俯仰角速率2°/s，抬头15秒，俯仰角到30°；保持此状态飞行20秒，飞机一直向北飞行；低头15秒，俯仰角至0°，期间飞机继续向北飞；
 *  保持向北飞行50秒；
 *  绕“8”字飞行； 即先左转一圈，后右转一圈；

左转一圈：rollleft 横滚角，持续4秒(此时一直向北飞)、turnleft转弯，持续180秒，转动360°；rollright横滚角，持续4秒，继续北向飞行；

右转一圈：rollright横滚角，持续4秒；turnright转弯，持续180秒，转动360°；rollleft横滚角，持续4秒，继续北向飞行；

*  继续向北飞行50秒；
 *  协调左转弯：rollleft横滚角，持续4秒(此时一直向北飞)；turnleft转弯90°，开始向西飞行，持续45秒；rollright横滚角，持续4秒，飞机向西飞行；
 *  以下实现两次循环：

(1) 保持向西飞行380秒;

(2) 协调右转弯：rollright横滚角，持续4秒；turnright转弯180°，持续90秒，开始向东飞行；rollleft横滚角，持续4秒，飞机向东飞行；

(3) 继续向东飞行380秒；

(4) 协调左转弯：rollleft横滚角，持续4秒(此时一直向东飞)；turnleft转弯180°，开始向西飞行，持续90秒；rollright横滚角，持续4秒，飞机向西飞行；

(5) 保持向西飞行380秒;

(6) 协调右转弯：rollright横滚角，持续4秒；turnright转弯180°，持续90秒，开始向东飞行；rollleft横滚角，持续4秒，飞机向东飞行；

(7) 继续向东飞行380秒；

(8) 协调左转弯：rollleft横滚角，持续4秒(此时一直向东飞)；turnleft转弯180°，开始向西飞行，持续90秒；rollright横滚角，持续4秒，飞机向西飞行；

*  协调左转弯：rollleft横滚角，持续4秒(此时一直向西飞)、turnleft转弯90°，开始向南飞行，持续45秒；rollright横滚角，持续4秒；
 *  向南飞行550秒；
 *  协调右转弯为：rollright横滚角，持续4秒；turnright转弯180°，持续105秒，开始向北飞行；rollleft横滚角，持续4秒；
 *  向北飞行50秒；
 *  俯仰角速率-2°/s，低头15秒，俯仰角到-30°;保持此状态20秒，飞机一直向北飞行；抬头15秒，俯仰角至0°，飞机继续向北飞；
 *  向北飞行30秒；
 *  向北飞行20秒，加速度为-5m/s^2；最后速度为0；
 *  保持静止状态，保证轨迹仿真时间为1小时；

轨迹仿真效果图：

（1）平面图：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70][]

（2）3维图：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1][]

航向角：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2][]

水平姿态角：

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3][]

### **二、POS融合：**test\_POS\_fusion.m ###

1、状态

% including: phi(3), dvn(3), dpos(3), eb(3)逐次启动零偏稳定性, db(3), lever(3), dT(1),  
% dKg(9), dKa(6), dvnGPS(3). (total states 6\*3+1+9+6+3=37)

kalman方程中对应的误差状态量为：phi(3), dvn(3), dpos(3), eb(3), db(3),lever(3), dT(1),

杆臂误差(3个)、时间延迟(1个)、存在于量测矩阵中；

[Psins_test_SINS_trj.m_test_SINS.m]: https://blog.csdn.net/wuwuku123/article/details/105311738
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70]: /images/20230528/59ea422b4b584c7b812f9b9fe9d0bdb4.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1]: /images/20230528/b951fc226a8e43019bb39f6c72bb1eba.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2]: /images/20230528/22885881f54641809701d5d153eebbca.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3]: /images/20230528/4f63bc17012541dea5a881f42e74b833.png