惯性导航(三)-基于流型的ESKF及代码实现

0. 专栏导读

惯性导航在SLAM技术的应用中有着举足轻重的作用,从刚体的姿态解算,到今年来常用的多传感器融合都离不开惯性导航,而惯导又涉及很多的理论知识,为此开设本专栏的旨在汇总惯性导航的知识点.
专栏涵盖以下几个方面:

  1. 从KF到EKF;
  2. 姿态解算;
  3. 基于流型(李代数)的ESKF及代码实现(本文);
  4. 预积分(待发布).

1. 引言

1.1 误差卡尔曼滤波

误差卡尔曼滤波(Error State Kalman Filter, ESKF)虽然从简称上看和EKF相似,但实质上还是有很大区别.无论KF还是EKF所关心的状态本身,而ESKF将注意力转到状态的误差,称为误差状态变量(Error State),而原来的状态称为名义状态比变量(Nominal State),名义状态变量和误差状态变量之和称为真值.

x t = x + δ x (1.1) \boldsymbol{x}_t = \boldsymbol{x} + \delta \boldsymbol{x} \tag{1.1} xt=x+δx(1.1)
上式中:

  • x t \boldsymbol{x}_t xt: 真值,右下角的t是指true,而不是时间
  • x \boldsymbol{x} x: 名义状态量
  • δ x \delta \boldsymbol{x} δx: 误差状态量

名义状态量不必考虑噪声,噪声在误差状态中考虑了.

1.2 ESKF的特点

相比传统的EKF,ESKF有以下优势:

  • ESKF总是在原点附近,离奇异点较远,数值方面更稳定,并且不会产生离工作点太远而导致线性化近似不够的问题;
  • ESKF的状态量为小量,其二阶变量相对来说可以忽略;同时,大多数雅可比矩阵在小量情况下变得非常简单,甚至可以用单位阵代替.

2. 数学推导

惯导系统中我们关心的状态有平移/速度/旋转,IMU的陀螺仪和加速度零偏服从随机游走模型,因此将其加入到状态中一并估计,另外也可以将重力g也放入到状态中(方便确定IMU的初始位姿,否则额需要实现确定初始时刻的IMU朝向),因此设状态变量如下:

x = [ p , v , q , b g , b a , g ] T (2.1) \boldsymbol{x} = [\boldsymbol{p}, \boldsymbol{v}, \boldsymbol{q}, \boldsymbol{b}_g, \boldsymbol{b}_a, \boldsymbol{g}]^T \tag{2.1} x=[p,v,q,bg,ba,g]T(2.1)

根据误差状态的定义有:
p t = p + δ p v t = v + δ v R t = R δ R or  q t = q δ q b g t = b g + δ b g b a t = b a + δ b a g t = g + δ g (2.2) \begin{aligned} &p_{t} =p+\delta\boldsymbol{p} \\ &\boldsymbol{v}_{t} =\boldsymbol{v}+\delta\boldsymbol{v} \\ &R_{t} =\boldsymbol{R}\delta\boldsymbol{R}\quad\text{or }\boldsymbol{q}_t=\boldsymbol{q}\delta\boldsymbol{q} \\ &\boldsymbol{b}_{gt} =\boldsymbol{b}_g+\delta\boldsymbol{b}_g \\ &\boldsymbol{b}_{at} =\boldsymbol{b}_a+\delta\boldsymbol{b}_a \\ &g_{t} =g+\delta\boldsymbol{g} \end{aligned} \tag{2.2} pt=p+δpvt=v+δvRt=RδRor qt=qδqbgt=bg+δbgbat=ba+δbagt=g+δg(2.2)

2.1 运动过程

和EKF一样,ESKF通过运动方程由上一时刻状态估计当前时刻,由于ESKF涉及误差状态和名义状态,因此不但需要知道名义状态的递推关系,还需要根据式2.2的定义推导误差状态的递推关系.

2.1.1 名义状态预测

首先处理名义状态对时间的导数,具体如下:
p ˙ = v v ˙ = R ( a ~ − b a ) + g R ˙ = R ( w ~ − b g ) ∧ b ˙ a = η b a b ˙ g = η b g g ˙ = 0 (2.3) \begin{align} \dot{\boldsymbol{p}} &= \boldsymbol{v} \\ \dot{\boldsymbol{v}} &= \boldsymbol{R}(\tilde{\boldsymbol{a}} - \boldsymbol{b}_a) + \boldsymbol{g} \\ \dot{\boldsymbol{R}} &= \boldsymbol{R}(\tilde{\boldsymbol{w}} - \boldsymbol{b}_g)^\wedge \\ \dot{\boldsymbol{b}}_a &= \eta_{ba} \\ \dot{\boldsymbol{b}}_g &= \eta_{bg} \\ \dot{\boldsymbol{g}} &= 0 \end{align} \tag{2.3} p˙v˙R˙b˙ab˙gg˙=v=R(a~ba)+g=R(w~bg)=ηba=ηbg=0(2.3)
由上面的时间导数以得离散时间的运动方程:
p ( t + Δ t ) = p ( t ) + v Δ t + 1 2 ( R ( a ~ − b a ) ) Δ t 2 + 1 2 g Δ t 2 v ( t + Δ t ) = v ( t ) + R ( a ~ − b a ) Δ t + g Δ t R ( t + Δ t ) = R ( t ) E x p ( ( ω ~ − b g ) Δ t ) b g ( t + Δ t ) = b g ( t ) b a ( t + Δ t ) = b a ( t ) g ( t + Δ t ) = g ( t ) (2.4) \begin{aligned} &\boldsymbol{p}(t+\Delta t) =\boldsymbol{p}(t)+\boldsymbol{v}\Delta t+\frac12(\boldsymbol{R}(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a)) \Delta t^2+\frac12\boldsymbol{g}\Delta t^2 \\ &\boldsymbol{v}(t+\Delta t) =\boldsymbol{v}(t)+\boldsymbol{R}(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a)\Delta t+\boldsymbol{g}\Delta t \\ &\boldsymbol{R}(t+\Delta t) =\boldsymbol{R}(t)\mathrm{Exp}\left((\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_g)\Delta t\right) \\ &\boldsymbol{b}_{g}(t+\Delta t) =\boldsymbol{b}_{g}(t) \\ &\boldsymbol{b}_{a}(t+\Delta t) =\boldsymbol{b}_{a}(t) \\ &\boldsymbol{g}(t+\Delta t) =\boldsymbol{g}(t) \end{aligned} \tag{2.4} p(t+Δt)=p(t)+vΔt+21(R(a~ba))Δt2+21gΔt2v(t+Δt)=v(t)+R(a~ba)Δt+gΔtR(t+Δt)=R(t)Exp((ω~bg)Δt)bg(t+Δt)=bg(t)ba(t+Δt)=ba(t)g(t+Δt)=g(t)(2.4)

2.1.2 误差状态预测

根据误差状态的定义可求得其连续时间的导数:
δ p ˙ = δ v δ v ˙ = − R ( a ~ − b a ) ∧ δ θ − R δ b a − η a + δ g δ θ ˙ = − ( ω ~ − b g ) ∧ δ θ − δ b g − η g δ b ˙ g = η b g δ b ˙ a = η b a δ g ˙ = 0 (2.5) \begin{aligned} \delta\dot{p} &=\delta\boldsymbol{v} \\ \delta\dot{v} &=-\boldsymbol{R}(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a)^\wedge\delta\boldsymbol{\theta}-\boldsymbol{R}\delta\boldsymbol{b}_a-\boldsymbol{\eta}_a+\delta\boldsymbol{g} \\ \delta\dot{\theta} &=-(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_g)^\wedge\delta\boldsymbol{\theta}-\delta\boldsymbol{b}_g-\boldsymbol{\eta}_g \\ \delta\dot{b}_{g} &=\eta_{bg} \\ \delta\dot{b}_{a} &=\eta_{ba} \\ \delta\dot{g} &=0 \end{aligned} \tag{2.5} δp˙δv˙δθ˙δb˙gδb˙aδg˙=δv=R(a~ba)δθRδbaηa+δg=(ω~bg)δθδbgηg=ηbg=ηba=0(2.5)
上式的推导只需明确以下两点:

  • 根据误差定义式2.2分别对两边求时间导数;
  • 名义状态和真值相同,只是不必考虑噪声

其中速度 v v v 和旋转 θ \theta θ 的推导过程较为复杂,需要用到SO3的伴随性质,详见高博的推导或书籍<自动驾驶与机器人中的SLAM技术-惯性导航与组合导航>
同理还需要求误差状态离散时间下的递推关系:

δ p ( t + Δ t ) = δ p + δ v Δ t δ v ( t + Δ t ) = δ v + ( − R ( a ~ − b a ) ∧ δ θ − R δ b a + δ g ) Δ t + η v δ θ ( t + Δ t ) = E x p ( − ( ω ~ − b g ) Δ t ) δ θ − δ b g Δ t − η θ δ b g ( t + Δ t ) = δ b g + η g δ b a ( t + Δ t ) = δ b a + η a δ g ( t + Δ t ) = δ g (2.6) \begin{aligned} &\delta\boldsymbol{p}(t+\Delta t) =\delta\boldsymbol{p}+\delta\boldsymbol{v}\Delta t \\ &\delta\boldsymbol{v}(t+\Delta t) =\delta\boldsymbol{v}+\left(-\boldsymbol{R}(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a)^{\wedge}\delta\boldsymbol{\theta}-\boldsymbol{R}\delta\boldsymbol{b}_a+\delta\boldsymbol{g}\right)\Delta t+\boldsymbol{\eta}_v \\ &\delta\boldsymbol{\theta}(t+\Delta t) =\mathrm{Exp}\left(-(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_g)\Delta t\right)\delta\boldsymbol{\theta}-\delta\boldsymbol{b}_g\Delta t-\boldsymbol{\eta}_\theta \\ &\delta\boldsymbol{b}_{g}(t+\Delta t) =\delta\boldsymbol{b}_g+\boldsymbol{\eta}_g \\ &\delta\boldsymbol{b}_{a}(t+\Delta t) =\delta\boldsymbol{b}_a+\boldsymbol{\eta}_a \\ &\delta\boldsymbol{g}(t+\Delta t) =\delta\boldsymbol{g} \end{aligned} \tag{2.6} δp(t+Δt)=δp+δvΔtδv(t+Δt)=δv+(R(a~ba)δθRδba+δg)Δt+ηvδθ(t+Δt)=Exp((ω~bg)Δt)δθδbgΔtηθδbg(t+Δt)=δbg+ηgδba(t+Δt)=δba+ηaδg(t+Δt)=δg(2.6)

回顾EKF,我们需要知道待估计状态在前一时刻到当前时刻的转换矩阵,而式2.6正是反映误差状态的转换关系,因此对于这里的ESKF而言,运动方程的雅可比矩阵为:

F = [ I I Δ t 0 0 0 0 0 I − R ( a ~ − b a ) ∧ Δ t 0 − R Δ t I Δ t 0 0 Exp ⁡ ( − ( ω ~ − b g ) Δ t ) − I Δ t 0 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] (2.7) F=\begin{bmatrix} I&I\Delta t&0&0&0&0\\ 0&I&-\boldsymbol{R}(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a)^{\wedge}\Delta t&0&-\boldsymbol{R}\Delta t&I\Delta t\\ 0&0&\operatorname{Exp}\left(-(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_g)\Delta t\right)&-\boldsymbol{I}\Delta t&0&0\\ 0&0&0&I&0&0\\ 0&0&0&0&I&0\\ 0&0&0&0&0&I \end{bmatrix} \tag{2.7} F= I00000IΔtI00000R(a~ba)ΔtExp((ω~bg)Δt)00000IΔtI000RΔt00I00IΔt000I (2.7)

为此,ESKF对误差状态的预测如下:
δ x p r e d = F δ x P p r e d = F P F T + Q \begin{align} \delta \boldsymbol{x}_{pred} &= \boldsymbol{F} \delta \boldsymbol{x} \tag{2.8a} \\ \boldsymbol{P}_{pred} &= \boldsymbol{F} \boldsymbol{P} \boldsymbol{F}^T + \boldsymbol{Q} \tag{2.8b} \end{align} δxpredPpred=Fδx=FPFT+Q(2.8a)(2.8b)
式2.8a实则意义不大,因为ESKF在误差状态每次更新以后会被重置 δ x = 0 \delta \boldsymbol{x} = 0 δx=0.因此预测过程主要是计算误差状态协方差.

2.2 更新过程

传统EKF中,根据观测方程的雅可比矩阵结合贝叶斯公式计算状态的后验.而ESKF中误差状态的后验直接对应名义状态的更新量,即
δ x = K ( z − h ( x ) ) x = x p r e d + δ x \begin{align} \delta \boldsymbol{x} &= \boldsymbol{K}\left(\boldsymbol{z}-h( \boldsymbol{x})\right) \tag{2.9a}\\ \boldsymbol{x}&=\boldsymbol{x}_{pred} + \delta \boldsymbol{x} \tag{2.9b} \end{align} δxx=K(zh(x))=xpred+δx(2.9a)(2.9b)
其中 K K K为增益:
K = P p r e d H T ( H P p r e d H T + V ) − 1 (2.10) \boldsymbol{K} = \boldsymbol{P}_{pred} \boldsymbol{H}^T (\boldsymbol{H}\boldsymbol{P}_{pred}\boldsymbol{H}^T+\boldsymbol{V})^{-1} \tag{2.10} K=PpredHT(HPpredHT+V)1(2.10)
误差状态的协方差更新为:
P = ( I − K H ) P p r e d (2.11) \boldsymbol{P}=(\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H})\boldsymbol{P}_{pred} \tag{2.11} P=(IKH)Ppred(2.11)

不难发现,上面式2.9-式2.11其实和传统的EKF的更新过程是完全一致,名义状态 x \boldsymbol{x} x 的调整量实质就是 δ x \delta\boldsymbol{x} δx, 但是,这里 H \boldsymbol{H} H 不再是观测方程的 h h h 关于 x \boldsymbol{x} x 的雅可比矩阵,而是关于 δ x \delta\boldsymbol{x} δx 的雅可比,通过链式法则有:
H = ∂ h ∂ δ x = ∂ h ∂ x ∂ x ∂ δ x (2.12) \boldsymbol{H} = \frac{\partial h}{\partial\delta\boldsymbol{x}} = \frac{\partial h}{\partial\boldsymbol{x}} \frac{\partial\boldsymbol{x}}{\partial\delta\boldsymbol{x}} \tag{2.12} H=δxh=xhδxx(2.12)
第一项和观测量有关,只需对观测方程线性化即可;第二项根据名义状态与误差状态定义(式2.2)不能得出:
∂ x ∂ δ x = d i a g ( I 3 , I 3 , ∂ L o g ( R E x p ( δ θ ) ) ∂ δ θ , I 3 , I 3 , I 3 ) (2.13) \frac{\partial\boldsymbol{x}}{\partial\delta\boldsymbol{x}}=diag(\boldsymbol{I}_3, \boldsymbol{I}_3, \frac{\partial Log(\boldsymbol{R}Exp(\delta \theta))}{\partial \delta \theta}, \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3) \tag{2.13} δxx=diag(I3,I3,δθLog(RExp(δθ)),I3,I3,I3)(2.13)
使用BCH近似可得:
∂ L o g ( R E x p ( δ θ ) ) ∂ δ θ = J r − 1 ( R ) (2.14) \frac{\partial Log(\boldsymbol{R}Exp(\delta \theta))}{\partial \delta \theta} = \boldsymbol{J}_r^{-1}(\boldsymbol{R}) \tag{2.14} δθLog(RExp(δθ))=Jr1(R)(2.14)

2.3 重置(option)

ESKF相比传统EKF还需要进行重置操作,这是因为误差状态的均值每一时刻结束时都被叠加到名义状态上(即式2.9b),对于下一时刻误差状态的先验均值应为0.引用高博的话说:

重置前,卡尔曼滤波刻画了 x p r e d \boldsymbol{x}_{pred} xpred 切空间处的分布 N ( δ x , P ) N(\delta\boldsymbol{x}, \boldsymbol{P}) N(δx,P), 重置后刻画的是 x p r e d + δ x \boldsymbol{x}_{pred} + \delta\boldsymbol{x} xpred+δx 处的分布 N ( 0 , P r e s e t ) N(0, \boldsymbol{P}_{reset}) N(0,Preset).对于本是是矢量的状态是无差的,但对于旋转变量来说,它们的切空间零点发生了改变,所以在数学习惯上,需要进行区分

用数学表达上面的过程如下:
R + E x p ( δ θ + ) = R k E x p ( δ θ k ) E x p ( δ θ + ) = R k E x p ( δ θ ) (2.15) \boldsymbol{R}^+Exp(\delta\boldsymbol{\theta}^+) = \boldsymbol{R}_k Exp(\delta\boldsymbol{\theta}_k)Exp(\delta\boldsymbol{\theta}^+) = \boldsymbol{R}_k Exp(\delta\boldsymbol{\theta}) \tag{2.15} R+Exp(δθ+)=RkExp(δθk)Exp(δθ+)=RkExp(δθ)(2.15)
上式中:

  • R k \boldsymbol{R}_k Rk: 重置前的名义旋转状态;
  • δ θ \delta\boldsymbol{\theta} δθ: 重置前的误差旋转状态;
  • δ θ k \delta\boldsymbol{\theta}_k δθk: 卡尔曼滤波更新后的误差旋转量;
  • R + \boldsymbol{R}^+ R+: 重置后的名义旋转状态;
  • δ θ + \delta\boldsymbol{\theta}^+ δθ+: 重置后的误差旋转状态.

切空间零点变化需要调整协方差,这里调整的方法和EKF运动方程/观测方程线性化的思想类似,都是通过泰勒展开近似成线性关系,重置前的"不确定性"通过该线性关系传递到重置后,这里的推导使用了BCH公式,这里同样不再详述这个过程,直接给出结论:
P r e s e t = J k P J k T J k = d i a g ( I 3 , I 3 , J θ , I 3 , I 3 , I 3 ) J θ = ∂ δ θ + ∂ δ θ = I − 1 2 δ θ k ∧ (2.16) \begin{align} \boldsymbol{P}_{reset} &= \boldsymbol{J}_k \boldsymbol{P} \boldsymbol{J}_k^T \\ \boldsymbol{J}_k &= diag(\boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{J}_{\theta}, \boldsymbol{I}_3, \boldsymbol{I}_3, \boldsymbol{I}_3) \\ \boldsymbol{J}_{\theta} &= \frac{\partial\delta\boldsymbol{\theta}^+}{\partial \delta \boldsymbol{\theta}} = \boldsymbol{I} - \frac{1}{2}\delta \boldsymbol{\theta}_k^{\wedge} \end{align} \tag{2.16} PresetJkJθ=JkPJkT=diag(I3,I3,Jθ,I3,I3,I3)=δθδθ+=I21δθk(2.16)

注意:

由于 δ θ k \delta\boldsymbol{\theta}_k δθk 并不大, J k \boldsymbol{J}_k Jk十分接近单位矩阵,所以大部分的材料并不处理这一项,而是直接把前面估计的 P \boldsymbol{P} P 矩阵作为下一时刻的协方差.

3. 代码实现

ESKF的C++实现可参考高博的<自动驾驶与机器人中的SLAM技术>书籍的配套源码(https://github.com/gaoxiang12/slam_in_autonomous_driving.git),下面对源码各部分对照上面数学推导作分析.

3.1 预测

template <typename S>
bool ESKF<S>::Predict(const IMU& imu) {
    ...
    // 对应式2.4
    VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
    VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
    SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);

    R_ = new_R;
    v_ = new_v;
    p_ = new_p;

    // 对应式2.7
    // F实际上是稀疏矩阵,也可以不用矩阵形式进行相乘而是写成散装形式,这里为了教学方便,使用矩阵形式
    Mat18T F = Mat18T::Identity();                                                 // 主对角线
    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 v
    F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对theta
    F.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 对 ba
    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 g
    F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 theta
    F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg

    // 对应式2.8a
    dx_ = F * dx_;  // 这行其实没必要算,dx_在重置之后应该为零,因此这步可以跳过,但F需要参与Cov部分计算,所以保留
    // 对应式2.8b
    cov_ = F * cov_.eval() * F.transpose() + Q_;
    ...
}

3.2 更新

以轮式编码器计算速度作为观测方程,更新过程如下:

template <typename S>
bool ESKF<S>::ObserveWheelSpeed(const Odom& odom) {
    assert(odom.timestamp_ >= current_time_);
    // 对应式2.12
    // 使用三维的轮速观测,由于这里观测量不涉及旋转,所以H较为简单,H为3x18,大部分为零
    Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
    H.template block<3, 3>(0, 3) = Mat3T::Identity();

    // 对应式2.10 卡尔曼增益
    Eigen::Matrix<S, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + odom_noise_).inverse();

    // compute velocity
    VecT vel_world = ...

    // 对应式2.9a
    dx_ = K * (vel_world - v_);

    // 对应式2.11 update cov
    cov_ = (Mat18T::Identity() - K * H) * cov_;

    UpdateAndReset();
    ...
}

void UpdateAndReset() {
    // 对应式2.9b
    p_ += dx_.template block<3, 1>(0, 0);
    v_ += dx_.template block<3, 1>(3, 0);
    R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));

    if (options_.update_bias_gyro_) {
        bg_ += dx_.template block<3, 1>(9, 0);
    }

    if (options_.update_bias_acce_) {
        ba_ += dx_.template block<3, 1>(12, 0);
    }

    g_ += dx_.template block<3, 1>(15, 0);

    ...
}

3.3 重置

重置协方差,对应式2.16

void ProjectCov() {
    Mat18T J = Mat18T::Identity();
    J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));
    cov_ = J * cov_ * J.transpose();
}

4. 小结

本文从理论到代码实现介绍了ESKF相关内容,整体而言,ESKF与传统的EKF有以下区别:

  1. 引入了误差状态和名义状态,名义状态可以理解为不考虑噪声的状态,真值与名义状态相差的正是误差状态;
  2. 运动预测过程除了预测名义状态外,还需要根据误差状态的定义计算其递推过程并预测;
  3. 观测过程的雅可比矩阵是关于误差状态本身的,而不是名义状态;
  4. 除了传统EKF的预测和更新两个步骤外,ESKF还需要进行重置操作,重置的目的是流型空间下状态的更新会影响其协方差,但是具体实现也是可以忽略.

另外,上面的旋转部分是以李代数表示并推导,实际上同样可以使用四元数表示并推导,日后再补充至本专栏.

  • 19
    点赞
  • 22
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
以下是基于被动时反的 ac-MVDR 近场声源定位的 MATLAB 代码示例: ```matlab % 近场声源定位仿真 % 基于被动时反的 ac-MVDR clear clc %% 参数设置 fs = 16000; % 采样率 c = 340; % 声速 M = 8; % 阵元数 d = 0.05; % 阵元间距 theta_s = 30; % 源角度 f = (0:1023)/1024*fs; % 频率范围 omega = 2*pi*f; % 角频率 k = omega/c; % 波数 %% 信号生成 t = (0:1999)/fs; s = sin(2*pi*1000*t) + sin(2*pi*2000*t); % 两个正弦信号混合 s = s(:); %% 阵列处理 theta = (-90:90); % 估计角度范围 theta = theta(:); theta_r = theta*pi/180; % 弧度制角度 a = exp(1j*(0:M-1)'*k*d*sin(theta_r)); % 阵列流型 x = zeros(M, length(s)); for ii = 1:length(theta) x(:, ii) = a(:, ii)*s; % 仿真接收信号 end %% 处理过程 Rxx = x*x'/size(x, 2); % 信号协方差矩阵 w = inv(Rxx)*a/(a'*inv(Rxx)*a); w_mvdr = w(:, 1); % MVDR权值 w_acmvdr = zeros(M, length(theta)); for ii = 1:length(theta) w_acmvdr(:, ii) = inv(Rxx + 1e-4*eye(M))*a(:, ii)/(a(:, ii)'*inv(Rxx + 1e-4*eye(M))*a(:, ii)); end power_spectrum = zeros(length(theta), length(f)); for ii = 1:length(theta) power_spectrum(ii, :) = abs(w_acmvdr(:, ii)'*a(:, ii)).^2; % 功率谱密度 end %% 结果可视化 figure(1) plot(theta, 10*log10(power_spectrum(:, end)), 'LineWidth', 2); hold on plot(theta_s*ones(size(theta)), -20:1:40, 'r--', 'LineWidth', 2); grid on xlabel('\theta (degree)', 'FontSize', 12, 'FontWeight', 'bold'); ylabel('Power spectrum (dB)', 'FontSize', 12, 'FontWeight', 'bold'); title('Acoustic source localization based on passive time reversal', 'FontSize', 14, 'FontWeight', 'bold'); legend('Estimated power spectrum', 'True source location', 'Location', 'northwest'); ``` 该代码实现了一个简单的基于被动时反的 ac-MVDR 近场声源定位仿真,其中使用了一个 8 阵元均匀线阵,采用两个正弦信号混合作为源信号,估计角度范围为 -90° 到 90°。运行该代码将得到一个图像,展示了估计的功率谱密度与真实源角度的对比。 需要注意的是,该代码仅作为示例,具体应用中可能需要根据实际情况进行调整和优化。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值