PRIDE PPP AR lsq浮点解 norm矩阵（未完，不定时更新。。。）

GNSS菜就多练

已于 2024-10-06 21:48:30 修改

阅读量989

点赞数 27

文章标签：矩阵线性代数

于 2024-10-05 22:07:15 首次发布

本文链接：https://blog.csdn.net/qq_39661203/article/details/142718765

版权

~~（客观评价 FORTRAN真垃圾）~~

NM%norm矩阵类似最小二乘结算 $B^{^{T}}PB$ ，是一个正定矩阵。

参数解释

NM%imtx -- NM%nc + NM%np 暂定表示估计参数个数（坐标钟差对流层无电离层组合所以不顾及电离层）

NM%nmtx = NM%nmtx + OB%amb_epo（OB%amb_epo为单个历元估计最多模糊度参数个数）

NM%norx --正则化矩阵

NM%iptp -- index point to parameters 指向参数索引

NM%iptx -- index point to x 指向待估参数索引

NM%ipm = NM%imtx -- index parameters m

NM%ltpl -- link to parameters l最小二乘残差平方和

NM%nuk = NM%nc 待定不清楚

NM%nobs = 0 多少个观测值

lsq.f90

首先在lsq_cnt_prmt.f90根据设置初始化部分NM的参数

!
!! lsq_cnt_prmt.f90
!!
!!    Copyright (C) 2023 by Wuhan University
!!
!!    This program belongs to PRIDE PPP-AR which is an open source software:
!!    you can redistribute it and/or modify it under the terms of the GNU
!!    General Public License (version 3) as published by the Free Software Foundation.
!!
!!    This program is distributed in the hope that it will be useful,
!!    but WITHOUT ANY WARRANTY; without even the implied warranty of
!!    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!!    GNU General Public License (version 3) for more details.
!!
!!    You should have received a copy of the GNU General Public License
!!    along with this program. If not, see <https://www.gnu.org/licenses/>.
!!
!! Contributor: Maorong Ge, Jianghui Geng, Songfeng Yang, Jihang Lin, Jing Zeng
!!
!!
!!
!!! purpose   : count number of the three types of parameters
!!              process, state and determinated parameters
!!
!
subroutine lsq_cnt_prmt(LCF, SITE, NM)
  implicit none
  include '../header/const.h'
  include '../header/station.h'
  include 'lsqcfg.h'
  include 'lsq.h'

  type(station) SITE
  type(lsqcfg) LCF
  type(norm) NM
!
!! local
  integer*4 i

  NM%nc = 0
  NM%np = 0
  NM%ns = 0
!
!! STATION PARAMETERS
!! We only estimate position, troposphere and clock parameters for stations
  if (SITE%skd(1:1) .eq. 'S' .or. SITE%skd(1:1) .eq. 'F') then
    NM%nc = NM%nc + 3
  else if (SITE%skd(1:1) .eq. 'P') then
    NM%np = NM%np + 3
  else if (SITE%skd(1:1) .eq. 'K') then
    NM%np = NM%np + 3
  else if (SITE%skd(1:1) .eq. 'L') then
    NM%np = NM%np + 3
  else
    write (*, '(2a,1x,a)') '***ERROR(lsq_cnt_prmt): unknown positioning mode ', SITE%name, SITE%skd
    call exit(1)
  end if
!
!! atmospheric parameters is process parameters
  if (LCF%ztdmod(1:3) .eq. 'PWC' .or. LCF%ztdmod(1:3) .eq. 'STO') then
    NM%np = NM%np + 1
  else if (LCF%ztdmod(1:3) .ne. 'NON') then
    write (*, '(2a)') '***ERROR(lsq_cnt_prmt): ztd mode ', LCF%ztdmod
    call exit(1)
  end if
  if (LCF%htgmod(1:3) .eq. 'PWC' .or. LCF%htgmod(1:3) .eq. 'STO') then
    NM%np = NM%np + 2
  else if (LCF%htgmod(1:3) .ne. 'NON') then
    write (*, '(2a)') '***ERROR(lsq_cnt_prmt): htg mode ', LCF%htgmod
    call exit(1)
  end if
!
!! receiver clock offset
  NM%np = NM%np + len_trim(LCF%sys)

  NM%imtx = NM%nc + NM%np
!
!! check consistence
  if (NM%imtx .gt. MAXPAR) then
    write (*, '(a,i8)') '***ERROR(lsq_cnt_prmt): too many parameters ', NM%imtx
    call exit(1)
  end if

  return
end subroutine

静态初始化NM%nc+3，其他模式NM%np+3（接收机X Y Z参数）

估计对流层NM%np+1

估计对流层梯度NM%np+2

估计接收机钟差（其实就是ISB）NM%np+LCF%sys（字符串类似GREH3）的非空格字符串长度

NM%imtx为NM%nc + NM%np 暂定表示估计参数个数（坐标钟差对流层无电离层组合所以不顾及电离层）

  NM%npm = 0
  NM%npm = NM%npm + OB%amb_tot
  NM%npm = NM%npm + NM%imtx
  if (NM%npm .gt. MAXPAR) then
    write (*, '(a,i8)') '***ERROR(lsq): too many parameters ', NM%npm
    call exit(1)
  end if
  allocate (PM(NM%npm), stat=ierr)
  if (ierr .ne. 0) then
    write (*, '(a,i8)') '***ERROR(lsq): parameter array allocation ', NM%npm
    call exit(1)
  end if

NM%npm｛number of parameters m｝初始化为0，之后添加整个历元所有卫星模糊度个数（tedit输出文件log***得到）,再加上估计参数个数

!! normal matrix size +1???
  if (LCF%lrmbias) then
    NM%nmtx = 0
    NM%nmtx = NM%nmtx + OB%amb_epo
    NM%nmtx = NM%nmtx + NM%imtx + 1
  else
    NM%nmtx = NM%npm + 1
  end if

!! allocate memory
  allocate (NM%norx(1:NM%nmtx, 1:NM%nmtx), stat=ierr)
  allocate (NM%iptp(NM%nmtx), stat=ierr)
  allocate (NM%iptx(NM%nmtx), stat=ierr)

  do i = 1, NM%nmtx
    NM%iptp(i) = 0
    NM%iptx(i) = NM%nmtx * (i - 1)
  end do

LCF%lrmbias对应：采用LAMBDA初始化NM%nmtx，NM%nmtx = NM%nmtx + OB%amb_epo（OB%amb_epo为单个历元估计最多模糊度参数个数）

NM%nmtx = NM%nmtx + NM%imtx + 1？？？不太理解为什么+1

之后初始化内存

进入lsq_init.f90

!
!! lsq_init.f90
!!
!!    Copyright (C) 2023 by Wuhan University
!!
!!    This program belongs to PRIDE PPP-AR which is an open source software:
!!    you can redistribute it and/or modify it under the terms of the GNU
!!    General Public License (version 3) as published by the Free Software Foundation.
!!
!!    This program is distributed in the hope that it will be useful,
!!    but WITHOUT ANY WARRANTY; without even the implied warranty of
!!    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!!    GNU General Public License (version 3) for more details.
!!
!!    You should have received a copy of the GNU General Public License
!!    along with this program. If not, see <https://www.gnu.org/licenses/>.
!!
!! Contributor: Maorong Ge, Jianghui Geng, Songfeng Yang, Jihang Lin, Jing Zeng
!!
!!
!!
!! purpose   : initialization of the least squares estimator
!! parameters:
!!             LCF   -- LSQ control struct
!!             SITE  -- station struct
!!             OB    -- observation struct
!!             NM,PM -- normal matrix & PAR table
!
subroutine lsq_init(LCF, SITE, OB, NM, PM)
  implicit none
  include '../header/const.h'
  include '../header/station.h'
  include '../header/rnxobs.h'
  include 'lsqcfg.h'
  include 'lsq.h'

  type(lsqcfg) LCF
  type(station) SITE
  type(rnxobr) OB
  type(norm) NM
  type(prmt) PM(1:*)
!
!! local
  integer*4 isat, ipar, irow
  integer*4 i, j, k, ic, ip, kpar, sec 
  character*4 sitename
  character*3 xyz, prnname
  character*2 htg
  character*6 sys
  real*8 val

  data xyz, htg, sys/'XYZ', 'CS', 'GREC3J'/
!
!! initialization
  do ipar = 1, NM%imtx + 1
    if (ipar .ne. NM%imtx + 1) then
      PM(ipar)%ptype = ' '
      PM(ipar)%ipt = 0
      PM(ipar)%iepo = 0
      PM(ipar)%iobs = 0
      PM(ipar)%iobs_G = 0
      PM(ipar)%iobs_R = 0
      PM(ipar)%iobs_E = 0
      PM(ipar)%iobs_C = 0
      PM(ipar)%iobs_3 = 0
      PM(ipar)%iobs_J = 0
      PM(ipar)%ptime(1) = LCF%jd0 + LCF%sod0/864.d2
      PM(ipar)%ptime(2) = LCF%jd1 + LCF%sod1/864.d2
      PM(ipar)%ptbeg = 0.d0
      PM(ipar)%ptend = 0.d0
      PM(ipar)%map = 0.d0
      PM(ipar)%rw = 0.d0
      PM(ipar)%xcor = 0.d0
      PM(ipar)%xsig = 0.d0
      PM(ipar)%xrms = 0.d0
      PM(ipar)%xrwl = 0.d0      ! especially Melbourne-Wubbena
      PM(ipar)%xswl = 0.d0
      PM(ipar)%mele = 0.d0
      PM(ipar)%zw = 0.d0        ! initial value of wide-lane
    end if
    do irow = 1, NM%imtx
      NM%norx(irow, ipar) = 0.d0
    end do
  end do

  do ipar = 1, MAXPAR_STA
    do isat = 1, MAXSAT
      OB%ltog(ipar, isat) = 0
    end do
  end do
  do isat = 1, MAXSAT
    OB%delay(isat) = 0.d0
    OB%flag(isat) = 0
  end do
  ic = 0
  ip = 0
  sec = 0
!
!! station depended
  OB%npar = 0
  if (SITE%skd(1:1) .eq. 'S' .or. SITE%skd(1:1) .eq. 'F') then
    do i = 1, 3
      ic = ic + 1
      ipar = ic
      PM(ipar)%pname = 'STAP'//xyz(i:i)
      PM(ipar)%ptype = 'C'
      PM(ipar)%ipt = ipar
      PM(ipar)%psat = 0
      PM(ipar)%xini = SITE%x(i)*1.d3
      OB%npar = OB%npar + 1
      OB%pname(OB%npar) = PM(ipar)%pname
      OB%ltog(OB%npar, 1) = ipar
      NM%norx(ipar, ipar) = 1.d0/SITE%dx0(i)**2
      NM%iptp(ipar) = ipar
    end do
  else if (SITE%skd(1:1) .eq. 'P') then
    do i = 1, 3
      ip = ip + 1
      ipar = NM%nc + ip
      write (PM(ipar)%pname, '(a,i0)') 'STAP'//xyz(i:i)//':', SITE%pospd
      PM(ipar)%ptype = 'P'
      PM(ipar)%ipt = ipar
      PM(ipar)%psat = 0
      PM(ipar)%xini = SITE%x(i)*1.d3
      PM(ipar)%ptime(1) = LCF%jd0 + LCF%sod0/864.d2 
      PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2 + SITE%pospd/864.d2 
      PM(ipar)%map = 0.d0
      PM(ipar)%rw = 1.d0/SITE%rx0(i)
      OB%npar = OB%npar + 1
      OB%pname(OB%npar) = PM(ipar)%pname
      OB%ltog(OB%npar, 1) = ipar
      NM%norx(ipar, ipar) = 1.d0/SITE%dx0(i)**2
      NM%iptp(ipar) = ipar
    end do
  else if (SITE%skd(1:1) .eq. 'K' .or. SITE%skd(1:1) .eq. 'L') then
    do i = 1, 3
      ip = ip + 1
      ipar = NM%nc + ip
      PM(ipar)%pname = 'STAP'//xyz(i:i)
      PM(ipar)%ptype = 'P'
      PM(ipar)%ipt = ipar
      PM(ipar)%psat = 0
      PM(ipar)%xini = 1.d3
      PM(ipar)%ptime(1) = LCF%jd0 + LCF%sod0/864.d2 
      PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2 
      PM(ipar)%map = 0.d0
      PM(ipar)%rw = 1.d0/SITE%dx0(i)
      OB%npar = OB%npar + 1
      OB%pname(OB%npar) = PM(ipar)%pname
      OB%ltog(OB%npar, 1) = ipar
      NM%norx(ipar, ipar) = 1.d0/SITE%dx0(i)**2
      NM%iptp(ipar) = ipar
    end do
  else
    write (*, '(2a,1x,a)') '***ERROR(lsq_init): unknown positioning mode ', SITE%name, SITE%skd
    call exit(1)
  end if
!
!! receiver clock offset
  do i = 1, 6
    if (index(LCF%sys, sys(i:i)) .ne. 0) then
      ip = ip + 1
      ipar = NM%nc + ip
      PM(ipar)%pname = 'RECCLK_'//sys(i:i)//'_'//trim(LCF%rckmod)
      PM(ipar)%ptype = 'P'
      PM(ipar)%ipt = ipar
      PM(ipar)%psat = 0
      if (LCF%rckmod(1:3) .eq. 'STO') then
        PM(ipar)%map = 1.d0
        PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2
        PM(ipar)%rw = 1.d0/(SITE%qrck*dsqrt(LCF%dintv/3600.d0))
      else if (LCF%rckmod(1:3) .eq. 'WNO') then
        PM(ipar)%map = 0.d0
        PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2
        PM(ipar)%rw = 1.d0/SITE%dclk0
      end if
      PM(ipar)%xini = 0.d0
      OB%npar = OB%npar + 1
      OB%pname(OB%npar) = PM(ipar)%pname
      do isat = 1, LCF%nprn
        if (LCF%prn(isat) (1:1) .eq. 'C') then
          if (sys(i:i) .eq. 'C') then
            read (LCF%prn(isat) (2:3), '(i2)') k
            if (k .le. 17) OB%ltog(OB%npar, isat) = ipar
          else if (sys(i:i) .eq. '3') then
            read (LCF%prn(isat)(2:3), '(i2)') k
            if (k .gt. 17) OB%ltog(OB%npar, isat) = ipar
          end if
          cycle
        end if
        if (LCF%prn(isat)(1:1) .eq. sys(i:i)) OB%ltog(OB%npar, isat) = ipar
      end do
      NM%norx(ipar, ipar) = 1.d0/SITE%dclk0**2
      NM%iptp(ipar) = ipar
    end if
  end do

!
!! disable atmosphere models for LEO satellites
  if (SITE%skd(1:1) .eq. 'L') goto 50

!
!! zenith atmospheric delay
  if (LCF%ztdmod(1:3) .ne. 'NON') then
    if (LCF%ztdmod(1:3) .eq. 'PWC') then
      k = index(LCF%ztdmod, ':')
      read (LCF%ztdmod(k + 1:), *) sec 
      sec = sec * 60
      kpar = nint(((LCF%jd1 - LCF%jd0)*864.d2 + (LCF%sod1 - LCF%sod0))/sec)
    end if
    ip = ip + 1
    ipar = NM%nc + ip
    if (LCF%ztdmod(1:3) .eq. 'STO') then
      PM(ipar)%pname = 'ZTDSTO'
    else if (LCF%ztdmod(1:3) .eq. 'PWC') then
      write (PM(ipar)%pname, '(a,i0)') 'ZTDPWC:', sec
    end if
    PM(ipar)%ptype = 'P'
    PM(ipar)%ipt = ipar
    PM(ipar)%psat = 0
    if (LCF%ztdmod(1:3) .eq. 'STO') then
      PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2
    else if (LCF%ztdmod(1:3) .eq. 'PWC' .and. kpar .gt. 1) then
      PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2 + sec/864.d2
    end if
    PM(ipar)%map = 1.d0
    if (LCF%ztdmod(1:3) .eq. 'STO') then
      PM(ipar)%rw = 1.d0/(SITE%qztd*dsqrt(LCF%dintv/3600.d0))
    else if (LCF%ztdmod(1:3) .eq. 'PWC') then
      PM(ipar)%rw = 1.d0/(SITE%qztd*dsqrt(sec/3600.d0))
    end if
    PM(ipar)%xini = 0.d0
    OB%npar = OB%npar + 1
    OB%pname(OB%npar) = PM(ipar)%pname
    OB%ltog(OB%npar, 1) = ipar
    NM%norx(ipar, ipar) = 1.d0/SITE%dztd0**2
    NM%iptp(ipar) = ipar
  end if

!
!! horizontal troposphere gradients
  if (LCF%htgmod(1:3) .ne. 'NON') then
    if (LCF%htgmod(1:3) .eq. 'PWC') then
      k = index(LCF%htgmod, ':')
      read (LCF%htgmod(k + 1:), *) sec
      sec = sec * 60 
      kpar = nint(((LCF%jd1 - LCF%jd0)*864.d2 + (LCF%sod1 - LCF%sod0))/sec)
    end if
    do i = 1, 2
      ip = ip + 1
      ipar = NM%nc + ip
      if (LCF%htgmod(1:3) .eq. 'STO') then
        PM(ipar)%pname = 'HTG'//htg(i:i)//'STO'
      else if (LCF%htgmod(1:3) .eq. 'PWC') then
        write (PM(ipar)%pname, '(a,i0)') 'HTG'//htg(i:i)//'PWC:', sec
      end if
      PM(ipar)%ptype = 'P'
      PM(ipar)%ipt = ipar
      PM(ipar)%psat = 0
      if (LCF%htgmod(1:3) .eq. 'STO') then
        PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2
      else if (LCF%htgmod(1:3) .eq. 'PWC' .and. kpar .gt. 1) then
        PM(ipar)%ptime(2) = LCF%jd0 + LCF%sod0/864.d2 + sec/864.d2
      end if
      PM(ipar)%map = 1.d0
      if (LCF%htgmod(1:3) .eq. 'STO') then
        PM(ipar)%rw = 1.d0/(SITE%qhtg*dsqrt(LCF%dintv/3600.d0))
      else if (LCF%htgmod(1:3) .eq. 'PWC') then
        PM(ipar)%rw = 1.d0/(SITE%qhtg*dsqrt(sec/43200.d0))
      end if
      PM(ipar)%xini = 0.d0
      OB%npar = OB%npar + 1
      OB%pname(OB%npar) = PM(ipar)%pname
      OB%ltog(OB%npar, 1) = ipar
      NM%norx(ipar, ipar) = 1.d0/SITE%dhtg0**2
      NM%iptp(ipar) = ipar
    end do
  end if

50 continue
  NM%ipm = NM%imtx

!
!! copy common parameter index
  do ipar = 1, OB%npar
    if (OB%pname(ipar) (1:6) .ne. 'RECCLK') then
      do isat = 2, LCF%nprn
        OB%ltog(ipar, isat) = OB%ltog(ipar, 1)
      end do
    end if
  end do
!
!! ambiguity (dynamic)
  OB%npar = OB%npar + 1
  OB%pname(OB%npar) = 'AMBC'
  do isat = 1, LCF%nprn
    OB%ltog(OB%npar, isat) = 0
    OB%lifamb(isat, 1) = 0.d0
    OB%lifamb(isat, 2) = 0.d0
  end do
!
!! check consistence
  if (ic .ne. NM%nc .or. ip .ne. NM%np .or. ic + ip .ne. NM%imtx) then
    write (*, '(a)') '***ERROR(lsq_init): parameter number not consistent with that from lsq_cnt_prmt'
    write (*, '(2(a,2i5,/))') ' nc, ic ', NM%nc, ic, ' np, ip ', NM%np, ip
    call exit(1)
  end if
!
!! output to screen
  write (*, '(a)') ' ALL PARAMETERS '
  do ipar = 1, NM%imtx
    val = NM%norx(ipar, ipar)
    sitename = SITE%name
    if (PM(ipar)%psat .eq. 0) then
      prnname = '   '
    else
      write (prnname, '(i3)') PM(ipar)%psat
    end if
    write (*, '(2i4,1x,a4,1x,a3,1x,a15)') ipar, PM(ipar)%psat, sitename, prnname, PM(ipar)%pname
  end do

  return
end subroutine

主要看NM参数，初始化NM%NM%imtx个PM，NM%norx初始矩阵大小（NM%imtx行 NM%imtx+1列）初始化矩阵的方差 NM%norx(ipar, ipar) = 1.d0/SITE%dx0(i)**2方差值为SITE%dx0(i)（config文件读取），同步更新 NM%iptp(ipar) = ipar，自后更新 NM%ipm = NM%imtx

NM%ltpl = 0.d0
  NM%nuk = NM%nc
  NM%nobs = 0

lsq_add_newamb.f90

添加模糊度参数，根据NM更新PM新加参数，更新NM%iptp

!! shift the rightsite to the last column
  do i = 1, NM%imtx + nbias
    if (i .gt. NM%imtx) then
      NM%norx(i, NM%imtx + 1 + nbias) = 0.d0
    else
      NM%norx(i, NM%imtx + 1 + nbias) = NM%norx(i, NM%imtx + 1)
    endif
  enddo

将莫名其妙加那一列移动到最后一列 i .gt. NM%imtx 将NM%imtx + 1 列移动到NM%imtx + 1 + nbias，i<NM%imtx设定为0 ，不知道为什么

!! clean the part for the new parameters
  do j = 1, nbias
    do i = 1, NM%imtx + j
      if (i .lt. NM%imtx + j) then
        NM%norx(i, NM%imtx + j) = 0.d0
      else if (i .eq. NM%imtx + j) then
        NM%norx(i, i) = 1.d-8
      endif
    enddo
  enddo

循环添加偏差个数

bias行上三角为0

bais对角线为-E-8

更新加单个历元模糊度个数

NM%ipm = NM%ipm + nbias

NM%imtx = NM%imtx + nbias

NM%ns = NM%ns + nbias

lsq_add_obs.f90

!! right hand side

NM%nobs = NM%nobs + 1

wrng = 1.d0/(OB%var(isat, 3) + OB%var(isat, 4))

range = (OB%omc(isat, 3)*r2(i0) - OB%omc(isat, 4))/(r2(i0) - 1.d0)

NM%nobs = NM%nobs + 1

wphs = 1.d0/(OB%var(isat, 1) + OB%var(isat, 2))

phase = (OB%omc(isat, 1)*r2(i0) - OB%omc(isat, 2))/(r2(i0) - 1.d0)

!更新nobs计算无电离层的omc（观测值减计算值）
if (OB%flag(isat) .eq. 1) then
          PM(ind)%xini = phase - range
          PM(ind)%zw = rwl
        end if
        phase = phase - amat(nelem)*PM(ind)%xini
初始化模糊度使用OMC相减得到，amat(nelem)模糊度的系数一般为1

do i = 1, nelem

do k = i, nelem

ir = min(ipt(i), ipt(k))

ic = max(ipt(i), ipt(k))

NM%norx(ir, ic) = NM%norx(ir, ic) + amat(i)*amat(k)*wphs

if (i .ne. ipt(0) .and. k .ne. ipt(0)) NM%norx(ir, ic) = NM%norx(ir, ic) + amat(i)*amat(k)*wrng

end do

ir = ipt(i)

NM%norx(ir, NM%imtx + 1) = NM%norx(ir, NM%imtx + 1) + amat(i)*wphs*phase

if (i .ne. ipt(0)) NM%norx(ir, NM%imtx + 1) = NM%norx(ir, NM%imtx + 1) + amat(i)*wrng*range

end do

NM%ltpl = NM%ltpl + wphs*phase**2

NM%ltpl = NM%ltpl + wrng*range**2

循环可用个数，只计算上三角矩阵，amat朴实无华变量命名（A矩阵，注意此时amat是转换到惯性坐标系下，并且存储为三个方向向量的和参考src/lsq/gpsmod.f90和src/lib/partial_gnss.f90），存储接收机卫星单位向量，这段代码就是计算 $B^{^{T}}PB$ （上三角），计算残差平方和