逆行列の計算(fortran)

今日は逆行列の計算をしてみたのでメモ。

ソルバはLAPACKのDGETRF,DGETRIで、fortran90。

! --------------------------------
! calculate inverse of matrix A(m,n)
! --------------------------------
function inv_lapack(A) result (AI)
    implicit none

    real(8), intent(in) :: A(:,:)
    real(8) :: AI(size(A,1),size(A,2))
    integer, parameter :: NB = 64
    integer :: info
    integer :: m, n

    ! for lapack
    integer :: LWORK
    integer, allocatable :: IPIV(:) ! dimension N
    real(8), allocatable :: WORK(:) ! dimension LWORK

    ! get size of A(m,n)
    m = size(A,1)
    n = size(A,2)

    LWORK = n*NB
    allocate(IPIV(n))
    allocate(WORK(LWORK))

    AI = A
    call DGETRF(m, n, AI, m, IPIV(1:min(m,n)), info)  ! factorize
    call DGETRI(n, AI, m, IPIV, WORK, LWORK, info)  ! inverse

    deallocate(IPIV)
    deallocate(WORK)

    if (INFO==0) then
        !write(*,*) "succeeded."
    endif

    return
end function

! --------------------------------
! show matrix A(m,n)
! --------------------------------
subroutine show_dmat(A,fmtf)
    implicit none
    real(8), intent(in) :: A(:,:)
    character(len=*), intent(in), optional :: fmtf
    character(len=25) :: fmtc
    integer :: m, n, i, j
    m = size(A,1); n = size(A,2)

    if (present(fmtf)) then
        fmtc = fmtf
    else
        fmtc = '(f13.4," ")'
    endif
    do i=1,m
        do j=1,n
            write(*,fmtc, advance='no') A(i,j)
        enddo
        write(*,*) ""
    enddo
    return
end subroutine show_dmat

呼び出しはこんな感じ。ついでにinterface付き。

program test
    implicit none

    interface
        function inv_lapack(A)
            real(8), intent(in) :: A(:,:)
            real(8) :: inv_lapack(size(A,2),size(A,1))
        end function inv_lapack

        subroutine show_dmat(A,fmtf)
            real(8), intent(in) :: A(:,:)
            character(len=*), intent(in), optional :: fmtf
        end subroutine show_dmat
    end interface

    real(8) :: A(2,2)
    real(8) :: AI(2,2)
    integer :: i, j

    A(1,1) = 1.d0
    A(1,2) = 2.d0
    A(2,1) = 4.5d0
    A(2,2) = 6.0d0

    write(*,'("A=")')
    call show_dmat(A, "(f9.3)")
    AI = inv_lapack(A)
    write(*,'("AI=")')
    call show_dmat(AI, "(f9.3)")

    stop
end program test

実行すると、まあうまくいったぽい。

$ gfortran -o test test.f90 -llapack
$ ./test
A=
    1.000    2.000 
    4.500    6.000 
AI=
   -2.000    0.667 
    1.500   -0.333 

QZSの軌道

前回作ってみたプログラムで８月１６日のQZSの軌道を描いてみた。
なぜかatanで計算するとyとzがプラマイ逆になってしまう時があったのでatan2を使って衛星軌道は計算した。なんでかな。
どうもc言語は使い慣れていないのがいけないなと思う。こんなことで時間を使ってしまうとは。

で、QZSを「小松」から見上げたら、こんな感じ。

おお、本当に８の字。

暦計算はGPSと全く同じだった。

放送暦の計算（２）

前に放送暦で衛星座標を計算したらRTKLIBのsatpossと100m以上の差が出てしまった。
（ちなみにQZS）

なんでかなと思っていたら、satposを使うと受信機から衛星までの伝搬時間分が補正されてしまうためだった。（前にメモした話のやつ）
単に時刻決め打ちで衛星座標を計算するなら、eph2posを使えば良い。

計算してみると、

------------------------------------------------------------------
2012/08/16 00:00:00 (x, y, z)
(自前)    -23318436.155264, 18586028.182482, -25160301.817461 
(eph2pos) -23318436.155244, 18586028.182504, -25160301.817463 (RTKLIB API) 
------------------------------------------------------------------

一応計算は合っているぽい。

この微妙な差はEkを計算する際のiteration方法で出るのだろうか？？

ちなみにEkの計算は、ただのiterationと、Newton-Raphson法の２種類試してみたのだけど、収束まで1,2回しか変わらなかった。

------------------------------------------------------------------
static double upd_E(const double E, const double M, const double e) {
    /* return M+e*sin(E); */ /* simple */
    return E-(E-M-e*sin(E))/(1-e*cos(E)); /* Newton-Raphson */
}
------------------------------------------------------------------

放送暦の計算

お盆が終わってから週末がなんだかんだと予定があり、何もできなかった。

今日は早めに帰ったし、久々にプログラムを書いてみることに。
ということでQZSSの放送暦から軌道を計算してみた。

とはいえ、IS-QZSSを読むのが面倒なのでIS-GPSの計算をやってみたのだけども。
(IS-GPS-200E, 8 June 2010 p.92-96)

で、自前で作ったプログラムと、RTKLIBで計算したQZSの座標値を比べると・・・

------------------------------------------------------------------
2012/08/16 00:00:00 (x, y, z)
(自前)    -23318436.155264, 18586028.182482, -25160301.817461 

(satpos) -23318319.272650, 18586114.815274, -25160339.595141 (RTKLIB API) 
------------------------------------------------------------------

なんかずいぶん違うよな・・・。100m超。

相対論効果を考慮してないのだけど、そのせいなのか、はたまた計算ミスなのか。
それとも計算式がGPSとQZSで違うのか。
ただ、酒を飲みながらプログラムを書いたので計算ミスの可能性が非常に高い。
いや、相対論効果って、ここで効いてくるのか？？どんどん分からなくなってきたぞ。
お酒怖い。

しかし、相対性理論が普通の生活で使われていて、こんなに影響してたら、すごいよなあ、と思う。一度まじめに相対性理論を勉強してみたい。

さて、衛星軌道の計算が大体合っていそうなら、あとは最小二乗で位置を計算すれば単独測位プログラムの完成だ。