捷联惯导频域优化算法和一种通用圆锥补偿系数计算程序

目录 * 1 数字滤波器 * 1.1 常用的数字滤波器 * 1.2 数字滤波设计 * 1.3 数字滤波举例 * 1.4 仿真结果分析 * 2 数字滤波在惯导算法中的应用 * 2.1 MEMS-IMU数字滤波 * 2.2 数字滤波对AHRS的影响 * 2.3 数字滤波对圆锥补偿的影响 * 3 圆锥误差补偿滤波优化算法 * 4 不同频率圆锥运动叠加的处理 * 5 圆锥系数检验 * 6 求解圆锥补偿系数的python程序 * 7 总结

1 数字滤波器

数字滤波器数非常重要的信号处理工具，自然也广泛应用在惯导系统设计中。数字滤波器和组合导航中的Kalman滤波器不同，前者的目的是用来过滤干扰频带的信号，后者主要是用来进行状态估计。

1.1 常用的数字滤波器

常见的数字滤波器类型包括低通滤波器、高通滤波器、带通滤波器和带阻滤波器。根据设计方法常见的可以分为FIR滤波器和IIR滤波器。

特性	FIR	IIR
实现方式	乘以系数	差分方程
相位特性	线性相位	可能非线性
稳定性	稳定	可能存在稳定性问题
计算量	较大	较小
设计灵活性	高	较高
应用场景	需要精确相位控制	对相位要求不高，追求效率

FIR滤波器具有严格的线性相位特性，这对于某些应用（如音频处理、图像处理中的边缘检测）非常重要。
IIR滤波器虽然设计简单、计算效率高，但其相位特性可能非线性，这在某些应用中是不可接受的。IIR滤波器由于存在反馈回路，可能会引入稳定性问题。相比之下，FIR滤波器总是稳定的。
FIR滤波器的计算量通常比IIR滤波器大，因为FIR滤波器需要更多的系数和乘法操作。然而，随着硬件性能的提升，这种差异在逐渐减少。
FIR滤波器的设计更加灵活，可以通过调整系数来改变滤波器的频率响应。IIR滤波器虽然设计相对简单，但在某些复杂应用中可能难以达到所需的性能。

1.2 数字滤波器设计

1.3 数字滤波举例

程序代码：filter_matlab_navspace

假设一个信号由5个不同频率的正弦信号复合而成，采样频率为400Hz，各个分量的频率为：
10Hz 30Hz 60Hz 100Hz 150Hz
在此基础上再加上两个不同方差不同的噪声：
randn(len,1).1; randn(len,1).5;

滤波前信号的频谱图

	幅频谱	功率谱
无噪声
噪声1
噪声5

滤波前的分量和复合信号
滤波后的分量个复合信号
滤波前后对比
滤波后数据的频谱分析滤波后的傅里叶谱图滤波后的功率谱图

1.4 数字滤波的特点

滤波器可以抑制干扰信号和噪声
滤波器对通带的信号同样会产生影响
经过滤波的信号会存在相位延迟

2 数字滤波在惯导算法中的应用

2.1 MEMS-IMU数字滤波

MEMS-IMU的数据到底需不需要进行滤波处理呢？这个问题不能一概而论，需要根据实际应用场景进行判断。无论是简单的航姿解算还是复杂的导航算法，其中的积分过程对信号有一定的平滑或滤波处理。MEMS器件的噪声较大且更加复杂，如果载体的动态范围确定，通过数字滤波器抑制干扰、降低噪声不失为一个好的选择。

2.2 数字滤波对AHRS的影响

2.3 数字滤波器对圆锥补偿的影响

3 圆锥误差补偿滤波优化算法

4 不同频率圆锥运动叠加的处理

5 利用符号运算求解圆锥系数的算法

6 求解圆锥补偿系数的python程序

from sympy import symbols, diff, sin, cos
import numpy as np

x, k1,k2,k3 = symbols('x k1 k2 k3')                # 定义符号变量
ss = 3              # 设置子样数
Fw = 0.9919068169085    # 设置滤波器增益
# Fw = 1
fun_left = (ss*x-(Fw**2)*sin(ss*x))         # 等式左式

if (ss==2):         # 2子样
    fun_right = 4*(Fw**2)*(1-cos(x))*(k1*sin(x))
elif(ss==3):        # 3子样
    fun_right = 4*(Fw**2)*(1-cos(x))*(k1*sin(x) + k2*sin(2*x))
elif(ss==4):        # 4子样
    fun_right = 4*(Fw**2)*(1-cos(x))*(k1*sin(x) + k2*sin(2*x) + k3*sin(3*x))
x_value = 0

fun_left_prime = fun_left       # 左侧公式求导
fun_right_prime = fun_right     # 右侧公式求导

Zk = []
Hk = []
for k in range(1,10):
    fun_left_prime = diff(fun_left_prime, x)
    fun_left_prime_value = fun_left_prime.subs(x, x_value)
    # print("{}阶的导数值： {}".format(k, fun_left_prime_value))

    fun_right_prime = diff(fun_right_prime, x)
    fun_right_prime_value = fun_right_prime.subs(x, x_value)
    # print("{}阶的导数值： {}".format(k, fun_right_prime_value))

    if (fun_right_prime_value != 0):
        print("{} = {}".format(fun_left_prime_value, fun_right_prime_value))
        Zk.append(fun_left_prime_value)
        if (ss==2):
            Hk.append(fun_right_prime_value.subs({k1:1}))
        elif (ss==3):
            Hk.append([fun_right_prime_value.subs({k1:1, k2:0}), fun_right_prime_value.subs({k1:0, k2:1})])
        elif (ss==4):
            Hk.append([fun_right_prime_value.subs({k1:1, k2:0, k3:0}), fun_right_prime_value.subs({k1:0, k2:1, k3:0}), fun_right_prime_value.subs({k1:0, k2:0, k3:1})])

Zk = np.array(Zk,dtype=float)
Hk = np.array(Hk,dtype=float)
Hk_t = np.transpose(Hk)
Xk = np.linalg.inv(Hk_t@Hk)@Hk_t@Zk

print("圆锥误差系数：{}".format(Xk))

以上程序的运行结果如下：

7 圆锥系数检验

8 总结

圆锥运动是一种理想的大机动，

References

[1]