当前位置:网站首页>Driverless learning (III): Kalman filter
Driverless learning (III): Kalman filter
2022-07-02 20:04:00 【biter0088】
1、 Write Kalman filter
Refer to the link below , Give comments on the basis of links , Write the Kalman filter as :kalmanfilter.h
#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H
#include "Eigen/Dense"
using namespace std;
class KalmanFilter{// Define a Kalman filter class
public:
//constructor: Is a method for creating and initializing “class Objects created ” Special method
KalmanFilter(){
is_initalized_= false;
};
//destructor Destructor , Contrary to constructors ;
// When an object ends its life cycle , When the function of the object has been called , The system automatically performs the destructor
//~KalmanFilter();
void Initalization(Eigen::VectorXd x_in){
x_= x_in;
is_initalized_= true;
//cout<<"intialized"<<endl;
}
bool IsInitalized(){
return is_initalized_;
}
Eigen::VectorXd GetX(){
return x_;
}
// State transition matrix F_( Set function ):state transistion matrix
// You can give it first F_in assignment , And then call SetF() Function pair state transition matrix F_ Assign a value
void SetF(Eigen::MatrixXd F_in){
F_ = F_in;
}
// State covariance matrix ( Set function ):state covariance matrix
void SetP(Eigen::MatrixXd P_in){
P_ = P_in;
}
// Process noise matrix ( Set function ):process covariance matrix
void SetQ(Eigen::MatrixXd Q_in){
Q_ = Q_in;
}
//x'= Fx + u. Next u Set to 0, That is, the external influence is ignored ( Simplify the model )
//P' = F*P*Ft + Q
void Prediction(){
//cout<<"prediction"<<endl;
x_ = F_ * x_;
//cout<<" Predictive value "<<x_<<endl;
//cout<<" Predictive value F "<<F_<<endl;
Eigen::MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;
}
// Measurement matrix :Measurement Matrix
void SetH(Eigen::MatrixXd H_in){
H_ = H_in;
}
// Measurement noise matrix :measurement covariance matrix
void SetR(Eigen::MatrixXd R_in){
R_ = R_in;
}
// forecast -- to update
// y=z-Hx' Actual observations z And the forecast x' Difference between y
// S=H*P*Ht+R S It's a temporary variable
// K=P'*Ht*S.inverse() K: Kalman gain , yes y The weight of the value
void Update(const Eigen::VectorXd &z){
// forecast
Eigen::VectorXd y = z - H_ * x_;
Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
Eigen::MatrixXd K = P_ * H_.transpose()*S.inverse(); // Kalman gain
// to update
x_ = x_+(K*y);// Update the current state vector
//cout<<" Update or get value "<<x_<<endl;
int size = x_.size();// Used to construct identity matrix
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
P_ = (I - K*H_)*P_;// Update the uncertainty of the system
}
// Defining parameters
private:
bool is_initalized_;// Whether to initialize
Eigen::VectorXd x_; //state vector State vector
Eigen::MatrixXd F_; // State transition matrix
Eigen::MatrixXd P_; // State covariance
Eigen::MatrixXd Q_; // noise
Eigen::MatrixXd H_; // Measurement matrix
Eigen::MatrixXd R_; // noise
};
#endif
Reference link :
边栏推荐
- Postman下载安装
- AcWing 1126. Minimum cost solution (shortest path Dijkstra)
- Génération automatique de fichiers d'annotation d'images vgg
- What is the Bluetooth chip ble, how to select it, and what is the path of subsequent technology development
- Codeforces Round #771 (Div. 2)(A-C)
- AcWing 341. Optimal trade solution (shortest path, DP)
- for(auto a : b)和for(auto &a : b)用法
- KS004 基于SSH通讯录系统设计与实现
- Automatic reading of simple books
- Embedded (PLD) series, epf10k50rc240-3n programmable logic device
猜你喜欢
sql-labs
【871. 最低加油次数】
Kt148a voice chip instructions, hardware, protocols, common problems, and reference codes
SQLite 3.39.0 release supports right external connection and all external connection
Data preparation for behavior scorecard modeling
Postman interface test practice, these five questions you must know
How to avoid duplicate data in gaobingfa?
【每日一题】241. 为运算表达式设计优先级
CRM客户关系管理系统
Shardingsphere jdbc5.1.2 about select last_ INSERT_ ID () I found that there was still a routing problem
随机推荐
pytorch 模型保存的完整例子+pytorch 模型保存只保存可訓練參數嗎?是(+解决方案)
Basic concept of database, installation and configuration of database, basic use of MySQL, operation of database in the project
Design and implementation of ks004 based on SSH address book system
Refactoring: improving the design of existing code (Part 2)
burp 安装 license key not recognized
从20s优化到500ms,我用了这三招
Exemple complet d'enregistrement du modèle pytoch + enregistrement du modèle pytoch seuls les paramètres d'entraînement sont - ils enregistrés? Oui (+ Solution)
Spark source code compilation, cluster deployment and SBT development environment integration in idea
Yes, that's it!
AcWing 181. Turnaround game solution (search ida* search)
Kt148a voice chip IC software reference code c language, first-line serial port
Cs5268 perfectly replaces ag9321mcq typec multi in one docking station solution
CRM Customer Relationship Management System
为什么我对流程情有独钟?
After 65 days of closure and control of the epidemic, my home office experience sharing | community essay solicitation
SQLite 3.39.0 release supports right external connection and all external connection
八年测开经验,面试28K公司后,吐血整理出高频面试题和答案
Use IDM to download Baidu online disk files (useful for personal testing) [easy to understand]
Summary of interview experience, escort your offer, full of knowledge points
After writing 100000 lines of code, I sent a long article roast rust