1.最小二乘lsq()/* 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 (nm) * 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 (wW^(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 (mn) //方程个数必须大于未知数的个数 return -1; Aymat(n,1); matmul(NN,n,1,m,1.0,A,y,0.0,Ay); /* AyA*y */ //计算法方程右边 matmul(NT,n,n,m,1.0,A,A,0.0,Q); /* QA*A */ //系数阵 if (!(infomatinv(Q,n))) matmul(NN,n,1,n,1.0,Q,Ay,0.0,x); /* xQ^-1*Ay */ //求解方程 free(Ay); return info; }2.卡尔曼滤波:filter_和filter/* kalman filter --------------------------------------------------------------- 卡尔曼滤波 * kalman filter state update as follows: * * KP*H*(H*P*HR)^-1, xpxK*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) * 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[ii*n] *-----------------------------------------------------------------------------*/ //仅rtkcmn.c文件可用 static 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) { /*kf核心函数*/ double *Fmat(n,m),*Qmat(m,m),*Kmat(n,m),*Ieye(n); //分配临时矩阵 K为增益矩阵 int info; matcpy(Q,R,m,m); //QR matcpy(xp,x,n,1); //xpp matmul(NN,n,m,n,1.0,P,H,0.0,F); /* QH*P*HR */ matmul(TN,m,m,n,1.0,H,F,1.0,Q); if (!(infomatinv(Q,m))) { matmul(NN,n,m,m,1.0,F,Q,0.0,K); /* KP*H*Q^-1 */ //计算卡尔曼增益 matmul(NN,n,1,m,1.0,K,v,1.0,xp); /* xpxK*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; } //自动筛选有效状态只更新非零且协方差为正的状态。 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; //筛选有效状态 iximat(n,1); for (ik0;in;i) if (x[i]!0.0P[ii*n]0.0) ix[k]i; //提取有效子矩阵 x_mat(k,1); //提取有效状态 xp_mat(k,1); P_mat(k,k); //提取有效协方差子矩阵 Pp_mat(k,k); //提取对应的H矩阵行 H_mat(k,m); for (i0;ik;i) { x_[i]x[ix[i]]; for (j0;jk;j) P_[ij*k]P[ix[i]ix[j]*n]; for (j0;jm;j) H_[ij*k]H[ix[i]j*n]; } infofilter_(x_,P_,H_,v,R,k,m,xp_,Pp_); //调用核心函数 //写回结果 for (i0;ik;i) { x[ix[i]]xp_[i]; for (j0;jk;j) P[ix[i]ix[j]*n]Pp_[ij*k]; } free(ix); free(x_); free(xp_); free(P_); free(Pp_); free(H_); return info; }3.平滑函数smoother()/* smoother -------------------------------------------------------------------- * 固定区间平滑器函数用于结合前向滤波和后向滤波的结果得到更优的平滑估计 * combine forward and backward filters by fixed-interval smoother as follows: * * xsQs*(Qf^-1*xfQb^-1*xb), Qs(Qf^-1Qb^-1)^-1) * * args : double *xf I forward solutions (n x 1) * args : double *Qf I forward solutions covariance matrix (n x n) * double *xb I backward solutions (n x 1) * double *Qb I backward solutions covariance matrix (n x n) * int n I number of solutions * double *xs O smoothed solutions (n x 1) * double *Qs O smoothed solutions covariance matrix (n x n) * return : status (0:ok,0:error) * notes : see reference [4] 5.2 * matirix stored by column-major order (fortran convention) *-----------------------------------------------------------------------------*/ extern int smoother(const double *xf, const double *Qf, const double *xb, const double *Qb, int n, double *xs, double *Qs) { double *invQfmat(n,n),*invQbmat(n,n),*xxmat(n,1); int i,info-1; matcpy(invQf,Qf,n,n); matcpy(invQb,Qb,n,n); if (!matinv(invQf,n)!matinv(invQb,n)) { for (i0;in*n;i) Qs[i]invQf[i]invQb[i]; if (!(infomatinv(Qs,n))) { matmul(NN,n,1,n,1.0,invQf,xf,0.0,xx); matmul(NN,n,1,n,1.0,invQb,xb,1.0,xx); matmul(NN,n,1,n,1.0,Qs,xx,0.0,xs); } } free(invQf); free(invQb); free(xx); return info; }