模糊度固定LAMBDA算法详解

最新推荐文章于 2024-02-22 16:44:54 发布

SHAO_GNSSer

最新推荐文章于 2024-02-22 16:44:54 发布

阅读量3.9k

点赞数 2

本文链接：https://blog.csdn.net/weixin_41854971/article/details/117374098

版权

算专栏收录该内容

7 篇文章 3 订阅

订阅专栏

文章目录

- 最小二乘搜索法

最小二乘搜索法

模糊度固定问题可以用一下方式表达：
$\check{a} = arg\quad min(a-\hat{a})^TQ_{\hat{a}}^{-1}(a-\hat{a})\tag{1}$
若 $Q_{\hat{a}}$ 为对角阵(即模糊度参数之间不相关)，直接对模糊度进行取整便可。因此必须对模糊度进行搜索。直接对模糊度进行搜索，其搜索空间十分狭长，一般采用降相关，再进行搜索。

LAMBDA搜索法采用Z变换的降相关方法：

$z=Z^Ta\\ \hat{z}=Z^T\hat{a}\\ Q_{\hat{z}}=Z^TQ_{\hat{a}}Z\tag{2}$
因此新的模糊度固定问题可转化为下列式子：
$\check{z} = arg\quad min(z-\hat{z})^TQ_{\hat{z}}^{-1}(z-\hat{z})\tag{3}$

1、LDL分解

$Q_{\hat{a}}=\bar{L}^T\bar{D}\bar{L}\tag{4}$
$Q_{\hat{z}}=L^TDL\tag{5}$
$L$ 矩阵对应的元素为 $l$ ， $\bar{L}$ 矩阵对应的元素为 $\bar{l}$ ，均为下三角矩阵

将(5)带入(3)可重写为：

$(z-\hat{z})^TL^{-T}D^{-1}L^{-1}(z-\hat{z})\tag{6}$

2、高斯变换

通过LDL分解的下三角矩阵 $L$ 中的元素并不为零，说明模糊度参数之间存在相关性，给定下三角矩阵 $L$ ，找出幺模矩阵 $Z$ 使得 $L Z$ 尽可能对角化，从而降低模糊度之间的相关性，使得通过 $Z$ 变换的模糊度相关性大大降低.

对 $L$ 下三角的元素 $l_{i,j}$ ,当 $l_{i,j}| > 0.5(j>i)$ 时，对应的高斯变换矩阵为：
$Z_{i,j}=I_n-[l_{i,j}]*e_ie_j^T\tag{7}$
其中， $I_n$ 为 $n$ 维单位向量， $[\cdot]$ 为取整算子， $e_i$ 和 $e_j$ 分别为 $i, j$ 对应的单位向量:
e.g.
$e_i = \left[ \begin{matrix} 0&0&···&1&0&···&0 \end{matrix} \right]$
1在第 $i$ 位。

$\bar{l}_{k,j}=l_{k,j}-[l_{i,j}] l_{k,i}\quad(k=i,i+1,···,n)\tag{8}$
举例：

给定单位下三角矩阵 $L$ 如下：

$\left[ \begin{matrix} 1&0&0&0\\ l_{21}&1&0&0\\ l_{31}&l_{32}&1&0\\ l_{41}&l_{42}&l_{43}&1\\ \end{matrix} \right]$
通过右乘高斯变换矩阵的方式，将 $L$ 尽可能对角化，其中[*]为取整操作， $\tilde{0}$ 表示接近零。
$LZ_{21}= \left[ \begin{matrix} 1&0&0&0\\ l_{21}&1&0&0\\ l_{31}&l_{32}&1&0\\ l_{41}&l_{42}&l_{43}&1\\ \end{matrix} \right] \left[ \begin{matrix} 1&0&0&0\\ -[l_{21}]&1&0&0\\ 0&0&1&0\\ 0&0&0&1\\ \end{matrix} \right] = \left[ \begin{matrix} 1&0&0&0\\ l_{21}-[l_{21}]*1&1&0&0\\ l_{31}-[l_{21}]*l_{32}&l_{32}&1&0\\ l_{41}-[l_{21}]*l_{42}&l_{42}&l_{43}&1\\ \end{matrix} \right]$
$LZ_{21}Z_{31}= \left[ \begin{matrix} 1&0&0&0\\ \tilde{0}_{21}&1&0&0\\ l_{31}^{(2)}&l_{32}^{(2)}&1&0\\ l_{41}^{(2)}&l_{42}^{(2)}&l_{43}^{(2)}&1\\ \end{matrix} \right] \left[ \begin{matrix} 1&0&0&0\\ 0&1&0&0\\ -[l_{31}^{(2)}]&0&1&0\\ 0&0&0&1\\ \end{matrix} \right] = \left[ \begin{matrix} 1&0&0&0\\ \tilde{0}_{21}&1&0&0\\ l_{31}^{(2)}-[l_{31}^{(2)}]*1&l_{32}^{(2)}&1&0\\ l_{41}^{(2)}-[l_{31}^{(2)}]*l_{43}^{(2)}&l_{42}^{(2)}&l_{43}^{(2)}&1\\ \end{matrix} \right]\\$
$. . .$
$\left[ \begin{matrix} 1&0&0&0\\ \tilde{0}_{21}&1&0&0\\ \tilde{0}_{31}&\tilde{0}_{32}&1&0\\ \tilde{0}_{41}&\tilde{0}_{42}&\tilde{0}_{43}&1\\ \end{matrix} \right], Z=Z_{21}Z_{31}Z_{32}Z_{41}Z_{42}Z_{43}= \left[ \begin{matrix} 1&0&0&0\\ -[l_{21}]&1&0&0\\ -[l_{31}^{(2)}]&-[l_{32}^{(3)}]&1&0\\ -[l_{41}^{(4)}]&-[l_{42}^{(5)}]&-[l_{43}^{(6)}]&1\\ \end{matrix} \right]$
码表示为：

输入：下三角矩阵L, 矩阵维度n
输出：变换矩阵Z
初始化：
    check(n >= 2)
    check(L ? unit lowr triangular)
开始：
    for row = 2 : n
        for col= 1 : row - 1
            u = [L(row, col)]
            if (u != 0)
                L(row : n, col) = L(row : n, col) - u * L(row: n, row)
                Z(row, col) = -u
            end
        end
    end

c代码如下

int8_t gauss_transform(matrix_t *L, matrix_t *Z)
{
    if (!lower_matrix_check(L))
    {
        return -1;
    }

    if (L->col <= 2)
    {
        return -1;
    }
    uint32_t col = 0;
    uint32_t row = 0;
    for (row = 1; row < L->row; ++row)
    {
        for (col = 0; col < row; ++col)
        {
            fp64 u = get_integer(L->element[row][col]);
            if (!IS_ZEROS(u))
            {
                Z->element[row][col] = -u;
                uint32_t i = 0;
                for (i = row; i < L->row; ++i)
                {
                    L->element[i][col] -= u * L->element[i][row];
                }
            }
        }
    }

    return 1;

}

3、条件方差排序

满足条件：

$d_i+\bar{l}_{i+1,i}^2d_{i+1} \leqslant d_{i+1}\tag{9}$
详情后续不上

4、确定模糊度的搜索空间

模糊度的搜索空间无法根据 $Q_{\hat{a}}$ 直接得到，所以我们定义：

$z-\tilde{z}=L(z-\hat{z})\tag{10}$
对(10)展开可得：

$\left[ \begin{matrix} z_1-\tilde{z}_1\\ ...\\ z_1-\tilde{z}_1 \end{matrix} \right]=\left[ \begin{matrix} 1&0&0&...\\ l_{21}&1&\cdot_{\cdot_{\cdot}}\\ ...&0&l_{n(n-1)}&1 \end{matrix} \right] \left( \left[ \begin{matrix} a_1\\ ...\\ a_n \end{matrix} \right] - \left[ \begin{matrix} \hat{a}_1\\ ...\\ \hat{a}_n \end{matrix} \right] \right)$
因此(6)可以更新为：

$F(\pmb{z})=(\pmb{z}-\tilde{\pmb{z}})^T\pmb{D}(\pmb{z}-\tilde{\pmb{z}}) =d_{11}(z_1-\tilde{z}_1)+···+d_{nn}(z_n-\tilde{z}_n) \leq \chi^2\tag{11}$
从(11)中可以得到 $\pmb{z}-\pmb{\tilde{z}}$ 的搜索空间：

$\left\{ \begin{aligned} |z_1-\tilde{z}_1| &= \sqrt{\frac{\chi^2}{d_{11}}}\\ |z_2-\tilde{z}_2| &= \sqrt{\frac{\chi^2 -(z_1-\tilde{z}_1)^2 d_{11}}{d_{22}}}\\ &···\\ |z_n-\tilde{z}_n| &= \sqrt{\frac{\chi^2 -[(z_1-\tilde{z}_1)^2 d_{11} + ···+(z_{n-1}+\tilde{z}_{n-1})^2 d_{(n-1)(n-1)}]}{d_{nn}}} \end{aligned} \right.\tag{12}$
将(10)代入(12)中：

$\left\{ \begin{aligned} &|z_1-\hat{z}_1| = \sqrt{\frac{\chi^2}{d_{11}}}\\ &|l_{21}(z_1-\hat{z}_1)+(z_2-\hat{z}_2)| = \sqrt{\frac{\chi^2 -(z_1-\hat{z}_1) d_{11}}{d_{22}}}\\ &···\\ &|l_{n1}(z_1-\hat{z}_1)+···+l_{n(n-1)}(z_{n-1}-\hat{z}_{n-1})+(z_n-\hat{z}_n)| = \sqrt{\frac{\chi^2 -[(z_1-\hat{z}_1)^2 d_{11} + (l_{21}(z_1-\hat{z}_1)+(z_{2}\hat{z}_{2}))^2d_{22} + ···]}{d_{nn}}} \end{aligned} \right.\tag{13}$
为了方便表达，我们有如下定义：

$\left\{ \begin{aligned} \bar{z}_1 &= \hat{z}_1\\ \bar{z}_2 &= \hat{z}_2 - l_{21}(z_1 - \hat{z}_1)\\ \bar{z}_3 &= \hat{z}_3 - l_{31}(z_1 - \hat{z}_1)- l_{32}(z_2-\hat{z}_2)\\ &···\\ \bar{z}_n &=\hat{z}_n - l_{n1}(z_1-\hat{z}_1)-l_{n2}(z_2-\hat{z}_2)-···-l_{n(n-1)}(z_{n-1}-\hat{z_{n-1}}) \end{aligned} \right.\tag{14}$
将(14)代入(13),则有：

$\left\{ \begin{aligned} &|z_1-\bar{z}_1| = \sqrt{\frac{\chi^2}{d_{11}}}\\ &|(z_2-\bar{z}_2)| = \sqrt{\frac{\chi^2 -(z_1-\bar{z}_1) d_{11}}{d_{22}}}\\ &···\\ &|(z_n-\bar{z}_n)| = \sqrt{\frac{\chi^2 -[(z_1-\bar{z}_1)^2 d_{11} + (z_2-\bar{z}_2)^2 d_{22}+···+(z_{n-1}-\bar{z}_{n-1})^2 d_{(n-1)(n-1)}]}{d_{nn}}} \end{aligned} \right.\tag{15}$
因此可以确定 $\pmb{z}$ 的搜索空间：

$\left\{ \begin{aligned} \bar{z}_1 - \sqrt{\frac{\chi^2}{d_{11}}} &\leq z_1 \leq \bar{z}_1 + \sqrt{\frac{\chi^2}{d_{11}}}\\ \bar{z}_2 - \sqrt{\frac{\chi^2 -(z_1-\bar{z}_1) d_{11}}{d_{22}}} &\leq z_2 \leq \bar{z}_2 + \sqrt{\frac{\chi^2 -(z_1-\bar{z}_1) d_{11}}{d_{22}}}\\ &···\\ \bar{z}_n - \sqrt{\frac{\chi^2 -[(z_1-\bar{z}_1)^2 d_{11} + (z_2-\bar{z}_2)^2 d_{22}+···+(z_{n-1}-\bar{z}_{n-1})^2 d_{(n-1)(n-1)}]}{d_{nn}}} &\leq z_n \leq \bar{z}_n + \sqrt{\frac{\chi^2 -[(z_1-\bar{z}_1)^2 d_{11} + (z_2-\bar{z}_2)^2 d_{22}+···+(z_{n-1}-\bar{z}_{n-1})^2 d_{(n-1)(n-1)}]}{d_{nn}}} \end{aligned} \right. \tag{16}$