C++代码实现MATLAB中的kalman函数功能-拓冰网站定制

#include<iostream>#include<vector>#include<cmath>#include<iomanip>#include<stdexcept>#include<algorithm>/* ==================== 极简矩阵类（纯标准库） ==================== */classMatrix{public:introws,cols;std::vector<double>data;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){}double&operator()(inti,intj){returndata[i*cols+j];}constdouble&operator()(inti,intj)const{returndata[i*cols+j];}staticMatrixidentity(intn){MatrixI(n,n);for(inti=0;i<n;++i)I(i,i)=1.0;returnI;}Matrixtranspose()const{MatrixT(cols,rows);for(inti=0;i<rows;++i)for(intj=0;j<cols;++j)T(j,i)=(*this)(i,j);returnT;}Matrixoperator+(constMatrix&o)const{MatrixR(rows,cols);for(size_t i=0;i<data.size();++i)R.data[i]=data[i]+o.data[i];returnR;}Matrixoperator-(constMatrix&o)const{MatrixR(rows,cols);for(size_t i=0;i<data.size();++i)R.data[i]=data[i]-o.data[i];returnR;}Matrixoperator*(constMatrix&o)const{if(cols!=o.rows)throwstd::runtime_error("Matrix dimension mismatch");MatrixR(rows,o.cols);for(inti=0;i<rows;++i)for(intk=0;k<cols;++k){doublea=(*this)(i,k);if(a==0.0)continue;for(intj=0;j<o.cols;++j)R(i,j)+=a*o(k,j);}returnR;}Matrixoperator*(doubles)const{MatrixR(rows,cols);for(size_t i=0;i<data.size();++i)R.data[i]=data[i]*s;returnR;}/* 高斯-约当消元法求逆（带部分选主元） */Matrixinverse()const{if(rows!=cols)throwstd::runtime_error("inverse: not square");intn=rows;Matrix A=*this;Matrix I=Matrix::identity(n);for(inti=0;i<n;++i){intpivot=i;for(intk=i+1;k<n;++k)if(std::fabs(A(k,i))>std::fabs(A(pivot,i)))pivot=k;if(std::fabs(A(pivot,i))<1e-12)throwstd::runtime_error("inverse: singular matrix");if(pivot!=i)for(intj=0;j<n;++j){std::swap(A(i,j),A(pivot,j));std::swap(I(i,j),I(pivot,j));}doubled=A(i,i);for(intj=0;j<n;++j){A(i,j)/=d;I(i,j)/=d;}for(intk=0;k<n;++k){if(k==i)continue;doublef=A(k,i);if(f==0.0)continue;for(intj=0;j<n;++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(constMatrix&A_,constMatrix&B_,constMatrix&C_,constMatrix&Q_,constMatrix&R_,constMatrix&x0,constMatrix&P0):A(A_),B(B_),C(C_),Q(Q_),R(R_),x(x0),P(P0){I=Matrix::identity(A.rows);}/* 预测步（等价于 MATLAB 的 predict） * x = A*x + B*u * P = A*P*A' + Q */Matrixpredict(constMatrix&u=Matrix()){x=A*x;if(u.rows>0&&B.cols>0)x=x+B*u;P=A*P*A.transpose()+Q;returnx;}/* 校正步（等价于 MATLAB 的 correct） * K = P*C'/(C*P*C'+R) * x = x + K*(z - C*x) * P = (I - K*C)*P */Matrixcorrect(constMatrix&z){Matrix S=C*P*C.transpose()+R;// 新息协方差Matrix K=P*C.transpose()*S.inverse();// 卡尔曼增益x=x+K*(z-C*x);// 更新状态P=(I-K*C)*P;// 更新协方差returnx;}};/* ==================== 使用示例 ==================== */intmain(){constintn=4;// 状态维数 [x, y, vx, vy]constintp=2;// 测量维数 [x, y]constdoubledt=0.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 的列数为 0）MatrixB(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 R=Matrix::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 P0=Matrix::identity(n)*10.0;// 构造滤波器KalmanFilterkf(A,B,C,Q,R,x0,P0);std::cout<<std::fixed<<std::setprecision(6);// 在线滤波：predict → correctfor(inti=0;i<100;++i){// 构造模拟测量值（真实轨迹：x = 1+0.5*t, y = 2+0.3*t）Matrixz(p,1);z(0,0)=1.0+0.5*i*dt;z(1,0)=2.0+0.3*i*dt;kf.predict();// 等价于 MATLAB: predict(kf)kf.correct(z);// 等价于 MATLAB: correct(kf, z)std::cout<<"Step "<<std::setw(3)<<i<<" x = ["<<kf.x(0,0)<<", "<<kf.x(1,0)<<", "<<kf.x(2,0)<<", "<<kf.x(3,0)<<"]\n";}return0;}
资讯详情

C++代码实现MATLAB中的kalman函数功能
