RTKLIB专题学习(三)
今天我们来进一步学习RTKLIB中矩阵的各种应用
rtkcmn.c : rtklib common functions
1.这是最小二乘法(实际在应用中为加权最小二乘)
-
m为观测方程数,n为待求参数,这里的A指的是设计矩阵的转置,y指的是观测方程
需要注意的是,RTKLIB在进行参数估计时,这里的y指的是观测残差,即y=v=l-HX,此时求出来的参数x实际上为x的增量
/* least square estimation -----------------------------------------------------
* least square estimation by solving normal equation (x=(A*A')^-1*A*y)
* args : double *A I transpose of (weighted) design matrix (n x m)
* double *y I (weighted) measurements (m x 1)
* int n,m I number of parameters and measurements (n<=m)
* double *x O estmated parameters (n x 1)
* double *Q O esimated parameters covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2))
* matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern int lsq(const double *A, const double *y, int n, int m, double *x,
double *Q)
{
double *Ay;
int info;
if (m<n) return -1;
Ay=mat(n,1);
matmul("NN",n,1,m,1.0,A,y,0.0,Ay); /* Ay=A*y */
matmul("NT",n,n,m,1.0,A,A,0.0,Q); /* Q=A*A' */
if (!(info=matinv(Q,n))) matmul("NN",n,1,n,1.0,Q,Ay,0.0,x); /* x=Q^-1*Ay */
free(Ay);
return info;
}
2.这是卡尔曼滤波
这里的X就是接收机位置,速度,加速度或者钟差,不再是最小二乘中的参数的增量,X表示参数矩阵,P表示参数的协方差矩阵
/* kalman filter ---------------------------------------------------------------
* kalman filter state update as follows:
*
* K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P
*
* args : double *x I states vector (n x 1)
* double *P I covariance matrix of states (n x n)
* double *H I transpose of design matrix (n x m)
* double *v I innovation (measurement - model) (m x 1)
* double *R I covariance matrix of measurement error (m x m)
double *Q - covariance matrix of v (m x m)
* int n,m I number of states and measurements
* double *xp O states vector after update (n x 1)
* double *Pp O covariance matrix of states after update (n x n)
*
* return : status (0:ok,<0:error)
* notes : matirix stored by column-major order (fortran convention)
* if state x[i]==0.0, not updates state x[i]/P[i+i*n]
*-----------------------------------------------------------------------------*/
extern int filter_(const double *x, const double *P, const double *H,
const double *v, const double *R, int n, int m,
double *xp, double *Pp)
{
double *F=mat(n,m),*Q=mat(m,m),*K=mat(n,m),*I=eye(n);
int info;
matcpy(Q,R,m,m);
matcpy(xp,x,n,1);
matmul("NN",n,m,n,1.0,P,H,0.0,F); /* Q=H'*P*H+R */
matmul("TN",m,m,n,1.0,H,F,1.0,Q);
if (!(info=matinv(Q,m))) {
matmul("NN",n,m,m,1.0,F,Q,0.0,K); /* K=P*H*Q^-1 */
matmul("NN",n,1,m,1.0,K,v,1.0,xp); /* xp=x+K*v */
matmul("NT",n,n,m,-1.0,K,H,1.0,I); /* Pp=(I-K*H')*P */
matmul("NN",n,n,n,1.0,I,P,0.0,Pp);
}
free(F); free(Q); free(K); free(I);
return info;
}
3.这是用于卡尔曼滤波前后的参数和协方差矩阵的更新函数
extern int filter(double *x, double *P, const double *H, const double *v,
const double *R, int n, int m)
{
double *x_,*xp_,*P_,*Pp_,*H_;
int i,j,k,info,*ix;
ix=imat(n,1); for (i=k=0;i<n;i++) if (x[i]!=0.0&&P[i+i*n]>0.0) ix[k++]=i;
x_=mat(k,1); xp_=mat(k,1); P_=mat(k,k); Pp_=mat(k,k); H_=mat(k,m);
for (i=0;i<k;i++) {
x_[i]=x[ix[i]];
for (j=0;j<k;j++) P_[i+j*k]=P[ix[i]+ix[j]*n];
for (j=0;j<m;j++) H_[i+j*k]=H[ix[i]+j*n];
}
info= filter_(x_,P_,H_,v,R,k,m,xp_,Pp_);
for (i=0;i<k;i++) {
x[ix[i]]=xp_[i];
for (j=0;j<k;j++) P[ix[i]+ix[j]*n]=Pp_[i+j*k];
}
free(ix); free(x_); free(xp_); free(P_); free(Pp_); free(H_);
return info;
}