#includeiostream#includevector#includecmath#includeiomanip#includestdexcept#includealgorithm/* 极简矩阵类纯标准库 */classMatrix{public:introws,cols;std::vectordoubledata;Matrix():rows(0),cols(0){}Matrix(intr,intc):rows(r),cols(c),data(r*c,0.0){}Matrix(intr,intc,doublev):rows(r),cols(c),data(r*c,v){}doubleoperator()(inti,intj){returndata[i*colsj];}constdoubleoperator()(inti,intj)const{returndata[i*colsj];}staticMatrixidentity(intn){MatrixI(n,n);for(inti0;in;i)I(i,i)1.0;returnI;}Matrixtranspose()const{MatrixT(cols,rows);for(inti0;irows;i)for(intj0;jcols;j)T(j,i)(*this)(i,j);returnT;}Matrixoperator(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]o.data[i];returnR;}Matrixoperator-(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]-o.data[i];returnR;}Matrixoperator*(constMatrixo)const{if(cols!o.rows)throwstd::runtime_error(Matrix dimension mismatch);MatrixR(rows,o.cols);for(inti0;irows;i)for(intk0;kcols;k){doublea(*this)(i,k);if(a0.0)continue;for(intj0;jo.cols;j)R(i,j)a*o(k,j);}returnR;}Matrixoperator*(doubles)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]*s;returnR;}/* 高斯-约当消元法求逆带部分选主元 */Matrixinverse()const{if(rows!cols)throwstd::runtime_error(inverse: not square);intnrows;Matrix A*this;Matrix IMatrix::identity(n);for(inti0;in;i){intpivoti;for(intki1;kn;k)if(std::fabs(A(k,i))std::fabs(A(pivot,i)))pivotk;if(std::fabs(A(pivot,i))1e-12)throwstd::runtime_error(inverse: singular matrix);if(pivot!i)for(intj0;jn;j){std::swap(A(i,j),A(pivot,j));std::swap(I(i,j),I(pivot,j));}doubledA(i,i);for(intj0;jn;j){A(i,j)/d;I(i,j)/d;}for(intk0;kn;k){if(ki)continue;doublefA(k,i);if(f0.0)continue;for(intj0;jn;j){A(k,j)-f*A(i,j);I(k,j)-f*I(i,j);}}}returnI;}};/* 卡尔曼滤波器 */classKalmanFilter{public:Matrix A,B,C,Q,R,x,P,I;KalmanFilter(constMatrixA_,constMatrixB_,constMatrixC_,constMatrixQ_,constMatrixR_,constMatrixx0,constMatrixP0):A(A_),B(B_),C(C_),Q(Q_),R(R_),x(x0),P(P0){IMatrix::identity(A.rows);}/* 预测步等价于 MATLAB 的 predict * x A*x B*u * P A*P*A Q */Matrixpredict(constMatrixuMatrix()){xA*x;if(u.rows0B.cols0)xxB*u;PA*P*A.transpose()Q;returnx;}/* 校正步等价于 MATLAB 的 correct * K P*C/(C*P*CR) * x x K*(z - C*x) * P (I - K*C)*P */Matrixcorrect(constMatrixz){Matrix SC*P*C.transpose()R;// 新息协方差Matrix KP*C.transpose()*S.inverse();// 卡尔曼增益xxK*(z-C*x);// 更新状态P(I-K*C)*P;// 更新协方差returnx;}};/* 使用示例 */intmain(){constintn4;// 状态维数 [x, y, vx, vy]constintp2;// 测量维数 [x, y]constdoubledt0.1;// 采样周期// 状态转移矩阵匀速运动模型MatrixA(n,n);A(0,0)1;A(0,2)dt;A(1,1)1;A(1,3)dt;A(2,2)1;A(3,3)1;// 无控制输入B 的列数为 0MatrixB(n,0);// 观测矩阵仅观测位置MatrixC(p,n);C(0,0)1.0;C(1,1)1.0;// 过程噪声作用在速度分量上MatrixQ(n,n);Q(2,2)0.1;Q(3,3)0.1;// 测量噪声Matrix RMatrix::identity(p)*0.5;// 初始状态估计与协方差Matrixx0(n,1);x0(0,0)3.0;x0(1,0)3.0;x0(2,0)0.0;x0(3,0)0.0;Matrix P0Matrix::identity(n)*10.0;// 构造滤波器KalmanFilterkf(A,B,C,Q,R,x0,P0);std::coutstd::fixedstd::setprecision(6);// 在线滤波predict → correctfor(inti0;i100;i){// 构造模拟测量值真实轨迹x 10.5*t, y 20.3*tMatrixz(p,1);z(0,0)1.00.5*i*dt;z(1,0)2.00.3*i*dt;kf.predict();// 等价于 MATLAB: predict(kf)kf.correct(z);// 等价于 MATLAB: correct(kf, z)std::coutStep std::setw(3)i x [kf.x(0,0), kf.x(1,0), kf.x(2,0), kf.x(3,0)]\n;}return0;}