工作记录——9

其他 2020-04-30 16:38:55 阅读次数: 0

Time ：2020.02.07

这是一篇实现线性卡尔曼滤波的算法。

  • 接下来任务：
  • [ 1 ]只是实现单步预测，接下来实现连续运行、预测；（已改动实现）
  • [ 2 ]非线性卡尔曼滤波
  • [ 3 ]此时采用伴随矩阵方法求逆矩阵，但是该方法效率较低，尤其在大型矩阵中，使用效果差，因此接下来要采用高斯-约旦方法实现矩阵求逆。

2020.02.12更新：
已经可以在类中实现卡尔曼滤波，将整个卡尔曼滤波的大程序更新于文末。

  • Inv.h和Inv.cpp ：实现矩阵取逆操作；

Inv.h

#ifndef INV_H
#define INV_H
#include <iostream>
#include <stdio.h>
#include <cstdio>
#include <vector>
#include <string>
#include "opencv2/highgui/highgui.hpp"

#define N 3    //测试矩阵维数定义
using namespace std;

class Inv{
public:
    Inv(){};//构造函数
    ~Inv(){};//析构函数
    double getA(vector<vector<double>> &arcs,int n);
    void  getAStart(vector<vector<double>> &arcs,int n,vector<vector<double>> &ans);
    vector<vector<double>> GetMatrixInverse(vector<vector<double>> &src,int n,vector<vector<double>> &des);//n是这个矩阵的行列数（可逆矩阵必定行=列）
};
#endif // INV_H

Inv.cpp

#include <Inv.h>
double Inv::getA(vector<vector<double>> &arcs,int n)
{
    if(n==1){
        return arcs[0][0];
    }
    double ans = 0;
    vector<vector<double>> temp(N,vector<double>(N));
    temp={{0,0,0},{0,0,0},{0,0,0}};
    int i,j,k;
    for(i=0;i<n;i++){
        for(j=0;j<n-1;j++){
            for(k=0;k<n-1;k++){
                temp[j][k] = arcs[j+1][(k>=i)?k+1:k];
            }
        }
        double t = getA(temp,n-1);
        if(i%2==0){
            ans += arcs[0][i]*t;
        }
        else{
            ans -=  arcs[0][i]*t;
        }
    }
    return ans;
}

//计算每一行每一列的每个元素所对应的余子式，组成A*
void  Inv::getAStart(vector<vector<double>> &arcs,int n,vector<vector<double>> &ans)
{
    if(n==1){
        ans[0][0] = 1;
        return;
    }
    int i,j,k,t;
    vector<vector<double>> temp(N,vector<double>(N));
    temp={{0,0,0},{0,0,0},{0,0,0}};

    for(i=0;i<n;i++){
        for(j=0;j<n;j++){
            for(k=0;k<n-1;k++){
                for(t=0;t<n-1;t++){
                    temp[k][t] = arcs[k>=i?k+1:k][t>=j?t+1:t];
                }
            }
            ans[j][i] = getA(temp,n-1);
//此处顺便进行了转置
           if((i+j)%2 == 1){
                ans[j][i] = - ans[j][i];
            }
        }
    }
}
//得到给定矩阵src的逆矩阵保存到des中。
/*bool Inv::GetMatrixInverse(vector<vector<double>> &src,int n,vector<vector<double>> &des)
{
    double flag=getA(src,n);
    //cout<<"flag "<<flag<<endl;
    vector<vector<double>> t(N,vector<double>(N));
    t={{0,0,0},{0,0,0},{0,0,0}};
    if(0==flag){
        cout<< "原矩阵行列式为0，无法求逆。请重新运行" <<endl;
        return false;//如果算出矩阵的行列式为0，则不往下进行
    }
    else{
        getAStart(src,n,t);
        for(int i=0;i<n;i++){
            for(int j=0;j<n;j++){
                des[i][j]=t[i][j]/flag;
            }
        }
    }
    return true;
}*/

vector<vector<double>> Inv::GetMatrixInverse(vector<vector<double>> &src,int n,vector<vector<double>> &des)
{
    double flag=getA(src,n);
    cout<<"flag "<<flag<<endl;
    vector<vector<double>> t(N,vector<double>(N));
    t={{0,0,0},{0,0,0},{0,0,0}};
    if(0!=flag){
        getAStart(src,n,t);
        for(int i=0;i<n;i++){
            for(int j=0;j<n;j++){
                des[i][j]=t[i][j]/flag;
            }
        }
         return des;//该成员函数的返回值是一个动态数组，因此无法返回return false；将return写在if判断语句的括号内，如果flag==0；则返回错误
    }
}
  • main函数 ：实现线性卡尔曼滤波，本打算在类中实现，但是一直没法输出结果。因为在KFP.h中无法直接定义vector<vector> statePre(n,vector(1))等类似的动态数组作为数据成员，导致在KFP.cpp中无法使用（初始化都不可以）。因此考虑将初始化函数void init(int &n,int &m)、预测函数vector<vector> predict()、校正函数vector<vector> correct(vector<vector>& measurementMatrix)写在main.cpp内。

最值得注意的是，过程噪声协方差和观测噪声协方差是对称矩阵，我在最开始假设数据时，就忽略了这点，导致数据输出一直不准确～

main.cpp

#include <iostream>
#include <stdio.h>
#include <cstdio>
#include <vector>
#include <string>
#include "opencv2/highgui/highgui.hpp"
#include <Inv.h>

using namespace std;
using namespace cv;

//n、m 是可以事先确定维数的
int n=3,m=3;
vector<vector<double>> statePre(n,vector<double>(1));            //< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)预测矩阵
vector<vector<double>> statePose(n,vector<double>(1)); //< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))最优估计矩阵
vector<vector<double>> transitionMatrix(n,vector<double>(n));   //< state transition matrix (A)状态转移矩阵
vector<vector<double>> mapMatrix(m,vector<double>(n));          //< measurement matrix (H)映射矩阵
vector<vector<double>> processNoiseCov(n,vector<double>(n));    //< process noise covariance matrix (Q)过程噪声
vector<vector<double>> measurementNoiseCov(m,vector<double>(m)); //< measurement noise covariance matrix (R)观测噪声
vector<vector<double>> erroCovPre(n,vector<double>(n));          //< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)  预测值与真实值的估计协方差矩阵erroCovPre
vector<vector<double>> gainMatrix(n,vector<double>(m));         //< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)卡尔曼增益
vector<vector<double>> erroCovPose(n,vector<double>(n));         //< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)最优估计协方差矩阵
vector<vector<double>> I(n,vector<double>(n));                   //  单位矩阵
//存放计算过程中产生的中间值的临时动态数组
vector<vector<double>> temp1(n,vector<double>(n));
vector<vector<double>> temp2(m,vector<double>(n));
vector<vector<double>> temp3(m,vector<double>(m));
vector<vector<double>> temp4(m,vector<double>(1));
vector<vector<double>> temp5(m,vector<double>(1));
vector<vector<double>> temp6(n,vector<double>(n));
vector<vector<double>> temp7(n,vector<double>(n));
vector<vector<double>> temp8(n,vector<double>(m));
vector<vector<double>> temp9(n,vector<double>(m));
vector<vector<double>> temp10(m,vector<double>(m));
vector<vector<double>> temp11(m,vector<double>(m));
vector<vector<double>> temp12(n,vector<double>(1));
vector<vector<double>> temp13(n,vector<double>(n));
vector<vector<double>> temp14(n,vector<double>(n));

/***********************************************************************************************************************************************************
KF实现流程：
1、i=0;初始化初始状态值，可以是x0，也可以是人为指定；初始化P0(人为设定)；
2、更新预测值statePre；（x(k)=A*x(k-1)+B*u(k)）
3、更新预测值与真实值的协方差矩阵erroCovPre；（P'(k)=A*P(k-1)*At + Q)）
4、更新卡尔曼增益矩阵gainMatrix；（K(k)=P'(k)*Ht/(H*P'(k)*Ht+R)）
5、更新最优估计值statePose；（x(k)=x'(k)+K(k)*(z(k)-H*x'(k))）
6、最优估计协方差矩阵erroCovPose；（P(k)=(I-K(k)*H)*P'(k)）
7、i=i+1，重复2-7步骤；

***********************************************************************************************************************************************************/
//实际上，在predict()中的for循环中,有很多动态数组也都进行赋值为0的操作，所以可不必在此处初始化，或者考虑所有的动态数组都在predict()中的for循环中实现赋值操作。

void init(int &n,int &m)//预测向量的元素数n和观测向量的元素数m，所有的矩阵维数都仅与此相关
{
//n*1
    for(int i=0;i<n;i++){
        for(int j=0;j<1;j++){
            statePre[i][j]=0;
            temp12[i][j]=0;
            //cout<<statePre[i][j]<<endl;
        }
    }
//n*n
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            erroCovPre[i][j]=0;
            erroCovPose[i][j]=0;
            temp1[i][j]=0;
            temp6[i][j]=0;
            temp7[i][j]=0;
            temp13[i][j]=0;
            temp14[i][j]=0;
            if(i==j){
                I[i][j]=1;
            }
            else
                I[i][j]=0;
        }
    }
//m*m
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            temp3[i][j]=0;
            temp10[i][j]=0;
            temp11[i][j]=0;
        }
    }
//n*m
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            gainMatrix[i].push_back(0);
            temp8[i][j]=0;
            temp9[i][j]=0;
        }
    }
//m*n
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            mapMatrix[i][j]=0;
            temp2[i][j]=0;

        }
    }
//定义一个单位矩阵I n*n
    I={{1,0,0},{0,1,0},{0,0,1}};
//m*1
    for(int i=0;i<m;i++){
        for(int j=0;j<1;j++){
            temp4[i][j]=0;
            temp5[i][j]=0;
        }
    }
}

vector<vector<double>> predict()
{
//更新预测值: x'(k)=A*x(k)     statePre = transitionMatrix*statePose;
    for(int i=0;i<n;i++){
        statePre[i][0]=0;//将statePre[i][0]初始化为0；否则一直是基于上次的ststepre的叠加
        for(int j=0;j<n;j++){
        statePre[i][0]+=transitionMatrix[i][j]*statePose[j][0];
        }
        cout<<"statePre "<<"["<<i<<"]"<<"[0] "<<statePre[i][0]<<endl;
    }

//更新预测值与真实值之间的协方差矩阵:分三步进行（左乘、求状态转移矩阵转置、右乘、Q）
    //左乘 temp1=A*P(k)
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            temp1[i][j]=0;
            for(int s=0;s<n;s++){
                temp1[i][j]+=transitionMatrix[i][s]*erroCovPose[s][j];
            }
        }
    }
    //求transitionMatrix矩阵的转置矩阵，temp6
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            temp6[j][i]=transitionMatrix[i][j];
         }
     }
    //右乘 P'(k) = temp1*temp6
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            temp7[i][j]=0;
            for(int s=0;s<n;s++){
                temp7[i][j]+=temp1[i][s]*temp6[s][j];
            }
        }
    }
    //Q过程噪声函数 errorCovPre = 1*temp1*transitionMatrix_t+1*processNoiseCov
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            erroCovPre[i][j]=temp7[i][j]+processNoiseCov[i][j];
        }
    }
    return statePre;
}

vector<vector<double>> correct(vector<vector<double>>& measurementMatrix)
{
//更新卡尔曼增益矩阵gainMatrix；（K(k)=P'(k)*Ht/(H*P'(k)*Ht+R)）分子temp9 分母temp10
    //求映射矩阵mapMatrix[m*n]的转置temp8[n*m](Ht)

    for(int i=0;i<m;i++){
        for(int j=0;j<n;j++){
            temp8[j][i]=mapMatrix[i][j];
         }
     }
    //求temp9[n*m]=P'(k)*Ht [n*n]*[n*m]
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            temp9[i][j]=0;
            for(int s=0;s<n;s++){
                temp9[i][j]+=erroCovPre[i][s]*temp8[s][j];
            }
        }
    }
    //求temp2[m*n]=H*P'(k)[m*n]*[n*n]
    for(int i=0;i<m;i++){
        for(int j=0;j<n;j++){
            temp2[i][j]=0;
            for(int s=0;s<n;s++){
                temp2[i][j]+=erroCovPre[i][s]*temp8[s][j];
            }
        }
    }
    //求temp3[m*m]=temp2*temp8[m*n]*[n*m]
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            for(int s=0;s<n;s++){
                temp3[i][j]+=temp2[i][s]*temp8[s][j];
            }
        }
    }
    //求temp10[m*m]=temp3+R
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            temp10[i][j]=temp3[i][j]+measurementNoiseCov[i][j];
        }
    }
    //求卡尔曼增益矩阵gainMatrix =temp9/temp10
        //求temp11=temp10_1（逆矩阵）
    Inv inv;
    inv.GetMatrixInverse(temp10,m,temp11);
        //求卡尔曼增益矩阵gainMatrix[n*m]=temp9*temp11 [n*m]*[m*m]
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
           gainMatrix[i][j]=0;
            for(int s=0;s<m;s++){
                gainMatrix[i][j]+=temp9[i][s]*temp11[s][j];
            }
        }
    }
//更新最优估计值statePose；（x(k)=x'(k)+K(k)*(z(k)-H*x'(k))）
    //求H*x'(k)===   temp5[m*1]=mapMatrix*statePre [m*n]*[n*1]
    for(int i=0;i<m;i++){
        for(int j=0;j<1;j++){
            temp5[i][j]=0;
            for(int s=0;s<n;s++){
                temp5[i][j]+=mapMatrix[i][s]*statePre[s][j];
            }
        }
    }
    //求temp4[m*1]= z(k)-H*x'(k))==measurementMatrix-temp5 [m*1]-[m*1]
    for(int i=0;i<m;i++){
        for(int j=0;j<1;j++){
            temp4[i][j]=measurementMatrix[i][j]-temp5[i][j];
        }
    }
    //求K(k)*(z(k)-H*x'(k))）==>temp12[n*1]=gainMatrix*temp4;  [n*m]*[m*1]
    for(int i=0;i<n;i++){
        for(int j=0;j<1;j++){
            temp12[i][j]=0;
            for(int s=0;s<m;s++){
                temp12[i][j]+=gainMatrix[i][s]*temp4[s][j];
            }
        }
    }
    //求最优估计值statePose[n*1]= statePre+temp12 [n*1]+[n*1]
    for(int i=0;i<n;i++){
        for(int j=0;j<1;j++){
            statePose[i][j]=statePre[i][j]+temp12[i][j];
             cout<<"statepose "<<statePose[i][j]<<endl;
        }
    }
//最优估计协方差矩阵erroCovPose；（P(k)=(I-K(k)*H)*P'(k)）
    //求K(k)*H===>temp13[n*n]=gainMatrix*mapMatrix  [n*m]*[m*n]
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            temp13[i][j]=0;
            for(int s=0;s<m;s++){
                temp13[i][j]+=gainMatrix[i][s]*mapMatrix[s][j];
            }
        }
    }
    //求I-K(k)*H===>temp14[n*n]=I-temp13 [n*n]-[n*n]
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            temp14[i][j]=I[i][j]-temp13[i][j];
        }
    }
    //求P(k)=(I-K(k)*H)*P'(k)）===>erroCovPose[n*n]=temp14*erroCovPre   [n*n]*[n*n]
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            erroCovPose[i][j]=0;
            for(int s=0;s<n;s++){
                erroCovPose[i][j]+=temp14[i][s]*erroCovPre[s][j];
            }
            //cout<<"erroCovPose "<<"["<<i<<"]"<<"["<<j<<"] "<<erroCovPose[i][j]<<endl;
        }
    }   
    return statePose;


}

int main(void)
{
    //输入测量值矩阵measurementMatrix m*1
    vector<vector<double>> measurementMatrix(3,vector<double>(1));
    vector<vector<double>> plus(3,vector<double>(1));
    measurementMatrix={{10},{10},{60}};
    plus={{0.8},{0},{0}};

    int a=3;
    int b=3;
    init(a,b);
    statePose={{15},{30},{85}};//n*1
    transitionMatrix={{1,0,0.01},{0,1,0},{0,0,1}};//n*n
    processNoiseCov={{0.1,0,0.05},{0,0.2,0.02},{0.05,0.02,0.2}};//n*n   对称矩阵（！！！）
    measurementNoiseCov={{0.05,0.01,0.02},{0.01,0,0},{0.02,0,0.1}};//m*m     对称矩阵（！！！）测量矩阵的协方差更小，更倾向于观测结果
    erroCovPose={{0.1,0,0.20},{0,0.1,0.1},{0,0,0.01}};
    mapMatrix={{1,0,0},{0,1,0},{0,0,1}};
    for(int z=0;z<5;z++){//进行五次连续预测更新最优估计值
        predict();
        correct(measurementMatrix);
        for(int p=0;p<3;p++){
            for(int b=0;b<1;b++){
                measurementMatrix[p][b]+=plus[p][b];
            }
        }
    }
    return 0;
}

2020.02.12更新：
KF.h

#ifndef KF_H
#define KF_H
#include <iostream>
#include <stdio.h>
#include <cstdio>
#include <vector>
#include <string>
//#include "opencv2/highgui/highgui.hpp"
//#include <Inv.h>
//#include <Invmatrix.h>
using namespace std;
//using namespace cv;
class KF{
  public:
    void init();
    vector<vector<double>> predict();
    vector<vector<double>> correct(vector<vector<double>>& measurementMatrix);
};

#endif // KF_H

KF.cpp

#include <KF.h>
#include <GJmatrix.h>

#define n 3
#define m 3
//n、m 是可以事先确定维数的
vector<vector<double>> statePre(n,vector<double>(1));            //< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)预测矩阵
vector<vector<double>> statePose(n,vector<double>(1)); //< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))最优估计矩阵
vector<vector<double>> transitionMatrix(n,vector<double>(n));   //< state transition matrix (A)状态转移矩阵
vector<vector<double>> mapMatrix(m,vector<double>(n));          //< measurement matrix (H)映射矩阵
vector<vector<double>> processNoiseCov(n,vector<double>(n));    //< process noise covariance matrix (Q)过程噪声
vector<vector<double>> measurementNoiseCov(m,vector<double>(m)); //< measurement noise covariance matrix (R)观测噪声
vector<vector<double>> erroCovPre(n,vector<double>(n));          //< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)  预测值与真实值的估计协方差矩阵erroCovPre
vector<vector<double>> gainMatrix(n,vector<double>(m));         //< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)卡尔曼增益
vector<vector<double>> erroCovPose(n,vector<double>(n));         //< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)最优估计协方差矩阵
vector<vector<double>> I(n,vector<double>(n));                   //  单位矩阵
vector<vector<double>> GJ_main_temp_one(n,vector<double>(n));
vector<vector<double>> GJ_main_temp_two(m,vector<double>(n));
vector<vector<double>> GJ_main_temp_three(m,vector<double>(m));
vector<vector<double>> GJ_main_temp_four(m,vector<double>(1));
vector<vector<double>> GJ_main_temp_five(m,vector<double>(1));
vector<vector<double>> GJ_main_temp_six(n,vector<double>(n));
vector<vector<double>> GJ_main_temp_seven(n,vector<double>(n));
vector<vector<double>> GJ_main_temp_eight(n,vector<double>(m));
vector<vector<double>> GJ_main_temp_nine(n,vector<double>(m));
vector<vector<double>> GJ_main_temp_ten(m,vector<double>(m));
vector<vector<double>> GJ_main_temp_eleven(m,vector<double>(m));
vector<vector<double>> GJ_main_temp_tweleve(n,vector<double>(1));
vector<vector<double>> GJ_main_temp_thirteen(n,vector<double>(n));
vector<vector<double>> GJ_main_temp_fourteen(n,vector<double>(n));
/*
KF实现流程：
1、i=0;初始化初始状态值，可以是x0，也可以是人为指定；初始化P0(人为设定)；
2、更新预测值statePre；（x(k)=A*x(k-1)+B*u(k)）
3、更新预测值与真实值的协方差矩阵erroCovPre；（P'(k)=A*P(k-1)*At + Q)）
4、更新卡尔曼增益矩阵gainMatrix；（K(k)=P'(k)*Ht/(H*P'(k)*Ht+R)）
5、更新最优估计值statePose；（x(k)=x'(k)+K(k)*(z(k)-H*x'(k))）
6、最优估计协方差矩阵erroCovPose；（P(k)=(I-K(k)*H)*P'(k)）
7、i=i+1，重复2-7步骤；
*/

void KF::init()//预测向量的元素数n和观测向量的元素数m，所有的矩阵维数都仅与此相关
{
//n*1
    for(int i=0;i<n;i++){
        for(int j=0;j<1;j++){
            statePre[i][j]=0;
            GJ_main_temp_tweleve[i][j]=0;
            //cout<<statePre[i][j]<<endl;
        }
    }
//n*n
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            erroCovPre[i][j]=0;
            erroCovPose[i][j]=0;
            GJ_main_temp_one[i][j]=0;
            GJ_main_temp_six[i][j]=0;
            GJ_main_temp_seven[i][j]=0;
            GJ_main_temp_thirteen[i][j]=0;
            GJ_main_temp_fourteen[i][j]=0;
            if(i==j){
                I[i][j]=1;
            }
            else
                I[i][j]=0;
        }
    }
//m*m
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_three[i][j]=0;
            GJ_main_temp_ten[i][j]=0;
            GJ_main_temp_eleven[i][j]=0;
        }
    }
//n*m
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            gainMatrix[i].push_back(0);
            GJ_main_temp_eight[i][j]=0;
            GJ_main_temp_nine[i][j]=0;
        }
    }
//m*n
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            mapMatrix[i][j]=0;
            GJ_main_temp_two[i][j]=0;
        }
    }
//定义一个单位矩阵I n*n
    I={{1,0,0},{0,1,0},{0,0,1}};
//m*1
    for(int i=0;i<m;i++){
        for(int j=0;j<1;j++){
            GJ_main_temp_four[i][j]=0;
            GJ_main_temp_five[i][j]=0;
        }
    }
    statePose={{12},{20},{65}};//n*1
    transitionMatrix={{1,0,0.01},{0,1,0},{0,0,1}};//n*n
    processNoiseCov={{0.1,0,0.05},{0,0.2,0.02},{0.05,0.02,0.2}};//n*n   对称矩阵（！！！）
    measurementNoiseCov={{0.05,0.01,0.02},{0.01,0,0},{0.02,0,0.1}};//m*m     对称矩阵（！！！）
    erroCovPose={{0.1,0,0.20},{0,0.1,0.1},{0,0,0.01}};
    mapMatrix={{1,0,0},{0,1,0},{0,0,1}};

}

vector<vector<double>> KF::predict()
{


//更新预测值: x'(k)=A*x(k)     statePre = transitionMatrix*statePose;
    for(int i=0;i<n;i++){
        statePre[i][0]=0;//将statePre[i][0]初始化为0；否则一直是基于上次的ststepre的叠加
        for(int j=0;j<n;j++){
        statePre[i][0]+=transitionMatrix[i][j]*statePose[j][0];
        }
        cout<<"statePre "<<"["<<i<<"]"<<"[0] "<<statePre[i][0]<<endl;
    }

//更新预测值与真实值之间的协方差矩阵:分三步进行（左乘、求状态转移矩阵转置、右乘、Q）
    //左乘 GJ_main_temp_one=A*P(k)
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_one[i][j]=0;
            for(int s=0;s<n;s++){
                GJ_main_temp_one[i][j]+=transitionMatrix[i][s]*erroCovPose[s][j];
                //cout<<"GJ_main_temp_one "<<"["<<i<<"]"<<"["<<j<<"] "<<GJ_main_temp_one[i][j]<<endl;
            }
        }
    }
    //求transitionMatrix矩阵的转置矩阵，GJ_main_temp_six
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_six[j][i]=transitionMatrix[i][j];
            //cout<<"GJ_main_temp_six "<<"["<<i<<"]"<<"["<<j<<"] "<<GJ_main_temp_six[i][j]<<endl;
         }
     }
    //右乘 P'(k) = GJ_main_temp_one*GJ_main_temp_six
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_seven[i][j]=0;
            for(int s=0;s<n;s++){
                GJ_main_temp_seven[i][j]+=GJ_main_temp_one[i][s]*GJ_main_temp_six[s][j];
            }
            //cout<<"GJ_main_temp_seven "<<"["<<i<<"]"<<"["<<j<<"] "<<GJ_main_temp_seven[i][j]<<endl;
        }
    }
    //Q过程噪声函数 errorCovPre = 1*GJ_main_temp_one*transitionMatrix_t+1*processNoiseCov
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            erroCovPre[i][j]=GJ_main_temp_seven[i][j]+processNoiseCov[i][j];
            //cout<<"erroCovPre "<<"["<<i<<"]"<<"["<<j<<"] "<<erroCovPre[i][j]<<endl;
        }
    }
    // handle the case when there will be measurement before the next predict.
    //statePre.copyTo(statePost);
    //errorCovPre.copyTo(errorCovPost);
    return statePre;
}
vector<vector<double>> KF::correct(vector<vector<double>>& measurementMatrix)
{
//更新卡尔曼增益矩阵gainMatrix；（K(k)=P'(k)*Ht/(H*P'(k)*Ht+R)）分子GJ_main_temp_nine 分母GJ_main_temp_ten
    //求映射矩阵mapMatrix[m*n]的转置GJ_main_temp_eight[n*m](Ht)

    for(int i=0;i<m;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_eight[j][i]=mapMatrix[i][j];
            //cout<<"mapMatrix "<<"["<<i<<"]"<<"["<<j<<"] "<<mapMatrix[i][j]<<endl;
            //cout<<"GJ_main_temp_eight "<<"["<<i<<"]"<<"["<<j<<"] "<<GJ_main_temp_eight[j][i]<<endl;
         }
     }
    //求GJ_main_temp_nine[n*m]=P'(k)*Ht [n*n]*[n*m]
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            GJ_main_temp_nine[i][j]=0;
            for(int s=0;s<n;s++){
                GJ_main_temp_nine[i][j]+=erroCovPre[i][s]*GJ_main_temp_eight[s][j];
                //cout<<"GJ_main_temp_eight "<<"["<<i<<"]"<<"["<<j<<"] "<<GJ_main_temp_eight[s][j]<<endl;
            }
            //cout<<"GJ_main_temp_nine "<<"["<<i<<"]"<<"["<<j<<"] "<<GJ_main_temp_nine[i][j]<<endl;
        }
    }
    //求GJ_main_temp_two[m*n]=H*P'(k)[m*n]*[n*n]
    for(int i=0;i<m;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_two[i][j]=0;
            for(int s=0;s<n;s++){
                GJ_main_temp_two[i][j]+=erroCovPre[i][s]*GJ_main_temp_eight[s][j];
            }
        }
    }
    //求GJ_main_temp_three[m*m]=GJ_main_temp_two*GJ_main_temp_eight[m*n]*[n*m]
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            for(int s=0;s<n;s++){
                GJ_main_temp_three[i][j]+=GJ_main_temp_two[i][s]*GJ_main_temp_eight[s][j];
            }
        }
    }
    //求GJ_main_temp_ten[m*m]=GJ_main_temp_three+R
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            GJ_main_temp_ten[i][j]=GJ_main_temp_three[i][j]+measurementNoiseCov[i][j];
            //cout<<"GJ_main_temp_ten "<<"["<<i<<"]"<<"["<<j<<"] "<<GJ_main_temp_ten[i][j]<<endl;
        }
    }
    //求卡尔曼增益矩阵gainMatrix =GJ_main_temp_nine/GJ_main_temp_ten
        //求GJ_main_temp_eleven=GJ_main_temp_ten_1（逆矩阵）
        
    // Inv是伴随矩阵的方式求可逆矩阵
    //Inv inv;
    //inv.GetMatrixInverse(GJ_main_temp_ten,m,GJ_main_temp_eleven);//输出结果传递给GJ_main_temp_eleven矩阵
    // Invmatrix是高斯-约旦消元的方式求可逆矩阵
    //Matrix D(3);
    //D.Input(GJ_main_temp_ten);
    //D.Cheak_IsSolvable();   //可解
    //D.Conversion();         //变换
    //GJ_main_temp_eleven=D.Out_Result();         //输出 将售出结果手动复制给GJ_main_temp_eleven
    // GJmatrix是精简版高斯-约旦消元的方式求可逆矩阵

    GJmatrix G;
    G.init(GJ_main_temp_ten);
    G.LU();//左下角置0
    G.check_matrix();
    G.IU();//对角线置1
    G.RU();//右上角置0
    GJ_main_temp_eleven=G.Result();//输出计算结果
        //求卡尔曼增益矩阵gainMatrix[n*m]=GJ_main_temp_nine*GJ_main_temp_eleven [n*m]*[m*m]
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
           gainMatrix[i][j]=0;
            for(int s=0;s<m;s++){
                gainMatrix[i][j]+=GJ_main_temp_nine[i][s]*GJ_main_temp_eleven[s][j];
                //cout<<"gainMatrix "<<"["<<i<<"]"<<"["<<j<<"] "<<gainMatrix[i][j]<<endl;
            }
        }
    }
//更新最优估计值statePose；（x(k)=x'(k)+K(k)*(z(k)-H*x'(k))）
    //求H*x'(k)===   GJ_main_temp_five[m*1]=mapMatrix*statePre [m*n]*[n*1]
    for(int i=0;i<m;i++){
        for(int j=0;j<1;j++){
            GJ_main_temp_five[i][j]=0;
            for(int s=0;s<n;s++){
                GJ_main_temp_five[i][j]+=mapMatrix[i][s]*statePre[s][j];
            }
        }
    }
    //求GJ_main_temp_four[m*1]= z(k)-H*x'(k))==measurementMatrix-GJ_main_temp_five [m*1]-[m*1]
    for(int i=0;i<m;i++){
        for(int j=0;j<1;j++){
            GJ_main_temp_four[i][j]=measurementMatrix[i][j]-GJ_main_temp_five[i][j];
        }
    }
    //求K(k)*(z(k)-H*x'(k))）==>GJ_main_temp_tweleve[n*1]=gainMatrix*GJ_main_temp_four;  [n*m]*[m*1]
    for(int i=0;i<n;i++){
        for(int j=0;j<1;j++){
            GJ_main_temp_tweleve[i][j]=0;
            for(int s=0;s<m;s++){
                GJ_main_temp_tweleve[i][j]+=gainMatrix[i][s]*GJ_main_temp_four[s][j];
            }
        }
    }
    //求最优估计值statePose[n*1]= statePre+GJ_main_temp_tweleve [n*1]+[n*1]
    for(int i=0;i<n;i++){
        for(int j=0;j<1;j++){
            statePose[i][j]=statePre[i][j]+GJ_main_temp_tweleve[i][j];
             cout<<"statepose "<<statePose[i][j]<<endl;
        }
    }

//最优估计协方差矩阵erroCovPose；（P(k)=(I-K(k)*H)*P'(k)）
    //求K(k)*H===>GJ_main_temp_thirteen[n*n]=gainMatrix*mapMatrix  [n*m]*[m*n]
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_thirteen[i][j]=0;
            for(int s=0;s<m;s++){
                GJ_main_temp_thirteen[i][j]+=gainMatrix[i][s]*mapMatrix[s][j];
            }
        }
    }
    //求I-K(k)*H===>GJ_main_temp_fourteen[n*n]=I-GJ_main_temp_thirteen [n*n]-[n*n]
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_main_temp_fourteen[i][j]=I[i][j]-GJ_main_temp_thirteen[i][j];
        }
    }
    //求P(k)=(I-K(k)*H)*P'(k)）===>erroCovPose[n*n]=GJ_main_temp_fourteen*erroCovPre   [n*n]*[n*n]
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            erroCovPose[i][j]=0;
            for(int s=0;s<n;s++){
                erroCovPose[i][j]+=GJ_main_temp_fourteen[i][s]*erroCovPre[s][j];
            }
            //cout<<"erroCovPose "<<"["<<i<<"]"<<"["<<j<<"] "<<erroCovPose[i][j]<<endl;
        }
    }
    return statePose;
}

GJmatrix.h

#ifndef GJMATRIX_H
#define GJMATRIX_H
#include <vector>
#include <string>
#include <iostream>
using namespace std;

class GJmatrix{
public:
    GJmatrix();
    ~GJmatrix();
    void init(vector<vector<double>> &A);//传入一个要求逆的矩阵,考虑要不要写入 矩阵维数
    bool LU();//左下角置0
    bool check_matrix();
    void IU();//对角线置1
    void RU();//右上角置0
    vector<vector<double>> Result();//输出计算结果
private:
    float Del_A;
};
#endif // GJMATRIX_H

GJmatrix.cpp

#include <GJmatrix.h>
#define n 3
//定义temp全局变量
vector<vector<double>> GJ_temp1(n,vector<double>(2*n));
vector<vector<double>> GJ_temp2(n,vector<double>(2*n));
vector<vector<double>> GJ_temp3(n,vector<double>(n));
//必须要先规定临时矩阵的维数，才可以对其进行初始化
//构造函数与析构函数
GJmatrix::GJmatrix(){}
GJmatrix::~GJmatrix(){}

void GJmatrix::init(vector<vector<double>> &A)//传入一个要求逆的矩阵,就将其重命名为A；新建立一个n*2n的动态数组GJ_temp1,并且其n-2n列开始，是单位矩阵；考虑要不要写入矩阵维数
{
//先给临时动态数组初始化
    //初始化时也要注意临时矩阵的行列数，之前因为忽略了GJ_temp3是n列，直接与1 2一起初始化，导致无法输出结果
    for(int i=0;i<n;i++){
        for(int j=0;j<2*n;j++){
            GJ_temp1[i][j]=0;
            GJ_temp2[i][j]=0;
        }
    }
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_temp3[i][j]=0;
        }
    }
    int p=n;
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            GJ_temp1[i][j]=A[i][j];
            GJ_temp1[i][p]=1;
        }
       p++;
    }
}
bool GJmatrix::LU()//左下角置0
{
     for(int i=0;i<n-1;i++){//没有处理i=n-1的值，但是在最后判断矩阵是否可逆的时候将其包含在内了
             if(GJ_temp1[i][i]){      //如果对角元素不为0 将其列下的元素全置为0
                 for(int m=i+1;m<n;m++){
                     if(GJ_temp1[m][i]){
                          float f=0;
                          f=-(GJ_temp1[m][i])/GJ_temp1[i][i];
                          //cout<<f<<endl;
                              for(int z=0;z<2*n;z++){
                                  GJ_temp1[m][z]=GJ_temp1[m][z]+f*GJ_temp1[i][z];
                              }
                          }
                     else continue;
                  }
             }
            //如果对角元素为0 寻找其下列不为零的行，并且交换行
            else{
                int m;
                for(m=i+1;m<n;m++){
                    if(GJ_temp1[m][i])
                        for(int z=0;z<2*n;z++){
                            GJ_temp2[i][z]=GJ_temp1[i][z];
                            GJ_temp1[i][z]=GJ_temp1[m][z];
                            GJ_temp1[m][z]=GJ_temp2[i][z];
                           // cout<<GJ_temp1[m][z]<<endl;
                        }
                    else continue;
               }
               if(m==n){//就是循环遍整个矩阵的列，m<n的都不满足非0条件，for循环再次执行 m++会等于n，超出数组的下标，因此返回错误，矩阵不可逆
               return false;
               }
               i--;//执行i--的原因在于：当执行else语句，说明交换了行，新的行含有GJ_temp1[i][i],仍然是第i行，但是执行完该for后，i会自动加1，因此在此处要减一
           }
    }
}
bool GJmatrix::check_matrix(){
    //因为上述GJmatrix::LU()的第一个for循环没有考虑最后一行的最后一个值，即GJ_temp1[n-1][n-1],因此在这里用Del_A判断是否对角元素的乘积为0
    Del_A=GJ_temp1[0][0];//不要写在for循环内部，不然会一直将del_a初始化为GJ_temp1[0][0]；
    for(int i=1;i<n;i++){
        Del_A=Del_A*GJ_temp1[i][i];
    }
   //cout<<"Del_A："<<Del_A<<endl;
   if(0==Del_A){
       return false;
   }
}
void GJmatrix::IU()//对角线置1
{
    if(check_matrix()){                          //如果矩阵有逆矩阵。说明对角线上元素都非0；那么接下来将对角线上的元素取反，其所在行的元素依次相乘这个系数即可
        for(int i=0;i<n;i++){
            float f=1/GJ_temp1[i][i];
               for(int j=0;j<2*n;j++){
               GJ_temp1[i][j]=f*GJ_temp1[i][j];
               }
        }
    }
}
void GJmatrix::RU()//右上角置0 从最后一列的倒数第二行开始
{
    for(int i=n-1;i>0;i--){
        for(int m=i-1;m>=0;m--){
            float f=-GJ_temp1[m][i];
            for(int j=0;j<2*n;j++){
                GJ_temp1[m][j]=GJ_temp1[m][j]+f*GJ_temp1[i][j];
            }
        }
    }

}
vector<vector<double>> GJmatrix::Result()//输出计算结果
{
    for(int i=0;i<n;i++){
        int p=0;//p一定不能放在for(int j=n;j<2*n;j++){}的内部，总犯这种错误
        for(int j=n;j<2*n;j++){
            GJ_temp3[i][p]=GJ_temp1[i][j];
            p++;
        }
    }
    return GJ_temp3;
}

main.cpp

#include <KF.h>

int main(void)
{
    //输入测量值矩阵measurementMatrix m*1
    vector<vector<double>> measurementMatrix(3,vector<double>(1));
    vector<vector<double>> plus(3,vector<double>(1));
    measurementMatrix={{10},{10},{60}};
    for(int p=0;p<3;p++){
        for(int b=0;b<1;b++){
            cout<<"measurementMatrix11[p][b] "<<"["<<p<<"]"<<"["<<b<<"] "<<measurementMatrix[p][b]<<endl;
        }
    }
    plus={{0.8},{0},{0}};//模拟观测值的变化，每10毫秒变化0.8米
    KF k;
    k.init();
    /*statePose={{12},{20},{65}};//n*1
    transitionMatrix={{1,0,0.01},{0,1,0},{0,0,1}};//n*n
    processNoiseCov={{0.1,0,0.05},{0,0.2,0.02},{0.05,0.02,0.2}};//n*n   对称矩阵（！！！）
    measurementNoiseCov={{0.05,0.01,0.02},{0.01,0,0},{0.02,0,0.1}};//m*m     对称矩阵（！！！）
    erroCovPose={{0.1,0,0.20},{0,0.1,0.1},{0,0,0.01}};
    mapMatrix={{1,0,0},{0,1,0},{0,0,1}};*/
    //while(1){
    for(int z=0;z<5;z++){
        k.predict();
        k.correct(measurementMatrix);
        for(int p=0;p<3;p++){
            for(int b=0;b<1;b++){
                measurementMatrix[p][b]+=plus[p][b];
                cout<<"measurementMatrix[p][b] "<<"["<<p<<"]"<<"["<<b<<"] "<<measurementMatrix[p][b]<<endl;
            }
        }
    }
    return 0;
}
罗宾酱
原创文章 15 获赞 8 访问量 917
关注 私信

猜你喜欢

转载自blog.csdn.net/weixin_39652282/article/details/104206659
今日推荐
周排行
Leetcode简单题61~80
解决zookeeper磁盘IO高的问题
多线程相关方法详解
Maven-setting.xml文件详解
Maven 项目的 classpath 理解
渊亭科技大数据笔试题
配置JVM内存分配
计算机网络个人学习笔记 （三）网络层 ：第三部分 连载
js中两个等号(==)和三个等号(===)的区别
用C程序自动打开电脑上的程序
每日归档
更多
2024-09-18(0)
2024-09-17(0)
2024-09-16(0)
2024-09-15(0)
2024-09-14(0)
2024-09-13(0)
2024-09-12(0)
2024-09-11(0)
2024-09-10(0)
2024-09-09(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022