實際工程項目中是怎么用卡爾曼濾波的?
共 15016字，需瀏覽 31分鐘
·
2024-04-19 10:20
點擊上方“小白學(xué)視覺”，選擇加"星標(biāo)"或“置頂”
重磅干貨，第一時間送達
編輯 | 汽車人
原文鏈接：https://www.zhihu.com/question/358334095
回答一
作者：李崇
鏈接：https://www.zhihu.com/question/358334095/answer/1160183841
兩大難題，一是運動學(xué)模型的建立，也就是預(yù)測方程。這個一方面可以通過比較細(xì)的系統(tǒng)辨識來做，這方面要結(jié)合具體的應(yīng)用背景，不一而足，也是普遍做的比較差的地方。
另一方面可用某個傳感器的輸出來做預(yù)測，比較典型的就是用加速度計做預(yù)測，再用陀螺做更新。
另一大問題就是噪聲的統(tǒng)計特性咯。因為建模誤差不知道。觀測方程到還可以測一下ZRO算一下均值和方差。然后下一步就是大家喜聞樂見的調(diào)參的階段咯，據(jù)說是群魔亂舞花樣百出。
我現(xiàn)實中只用過mems加速度計和陀螺的融合，還有就是無模型的跟蹤咯。也就是預(yù)測方程是0，純靠建模誤差的那個方差來做預(yù)測，效果咋說呢，穩(wěn)態(tài)下看噪聲統(tǒng)計特性是好了些，但是動態(tài)特性明顯滯后和變形了。應(yīng)該是我經(jīng)驗不足吧
回答二
作者：雁集
鏈接：https://www.zhihu.com/question/358334095/answer/1009240518
先啰嗦一下在工程中卡爾曼濾波要解決的問題是什么。
我們得到了2（或多個）個反饋同一件事的信息A和B，但不知道該相信哪個結(jié)果，所以簡單的，將兩個信息相加除以2（加權(quán)平均），得到一個結(jié)果0.5A+0.5B。但這個方法過于粗暴，不能反映出水平?，F(xiàn)在知道了其中一個信息可靠度更好，而另一個可靠度不好，就要更相信好的信息，如，現(xiàn)在給可信度好的B更大的權(quán)重，得到0.2A+0.8B，這個結(jié)果會好于第一個。但仍然覺得low，因為A與B的參數(shù)是根據(jù)經(jīng)驗設(shè)置的，并不知道是否是最合理的，是否是最優(yōu)化的。所以高級的，用優(yōu)化的方法，根據(jù)A與B的高斯分布特征，去融合A和B信息，這個就是卡爾曼。
所以，我們看到，要利用卡爾曼，需要不同的信息源（大于等于2個）。最常見的信息源是什么？就是我們根據(jù)系統(tǒng)模型或動力學(xué)模型計算出來的結(jié)果，可以通過仿真的形式得到。第二個或第三個信息是什么？在飛行器姿態(tài)估計，導(dǎo)航定位，目標(biāo)跟蹤（幾個問題的本質(zhì)一樣，都是狀態(tài)估計）的問題中，這個信息一般來自慣導(dǎo)或GPS或雷達等。有了2個信息數(shù)據(jù)，就可以根據(jù)Kalman的公式進行融合。下面舉一個機器人運動估計的例子，來自《Probabilistic Robotics》。
首先假設(shè)你的機器人長這樣（一個圓，比較簡單），在二維笛卡爾坐標(biāo)系中運動。
根據(jù)這個圖，可以得到它的運動學(xué)模型
得到以上模型還不能直接用，這個模型有變量的乘積和三角函數(shù)，是個非線性模型，但要利用卡爾曼公式，需要滿足EKF（實際工程應(yīng)用中，我還沒見過直接用KF的）的線性化條件。所以要將非線性的模型線性化。
到這一步，得到的Gt可以直接代入EKF公式去更新協(xié)方差矩陣了，經(jīng)過推算會得到增益矩陣K。EKF公式包括預(yù)測與更新，這里就不啰嗦了。
同時，用狀態(tài)的迭代形式，可以得到初步推算的位置。
最后，EKF公式中有一項
其中兩個z，代表了2個信息，比如其中一個是通過動力學(xué)得到的機器人位置，另一個通過雷達得到的機器人位置。
通過以上的計算，會得到一個靠譜的結(jié)果。
Kalman的其它公式都是用優(yōu)化理論去解算高斯函數(shù)的，在此不啰嗦了。
回答三
作者：虛函數(shù)機器人
鏈接：https://www.zhihu.com/question/358334095/answer/2396600132
關(guān)于卡爾曼原理的講解知乎上有太多了，而且理解的都比我好，在這里我就不班門弄斧了。我實現(xiàn)了一個融合慣導(dǎo)的加速度計和陀螺儀的簡易卡爾曼濾波器，代碼寫的十分簡單，保證初學(xué)者一看就懂，對那些復(fù)現(xiàn)算法有困難的同學(xué)會起到一定幫助。話不多說，上干貨： 會用到的卡爾曼腦仁疼公式如下：
加速度計數(shù)學(xué)模型滿足：
代碼融合如下，代碼風(fēng)格不好，各位別學(xué)我。（寫這個代碼的時候我還未成年）：
#include "imu.h"
#include <iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<string>
#include<vector>
#include <cmath>
#include <limits>//用于生成隨機分布數(shù)列
#include <stdlib.h>
#include <random>
#include <fstream>
#include <cstdlib> // 標(biāo)準(zhǔn)庫
using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;
int t=1;
double theta_PI=57.29578;
/* 定義系數(shù)*/
MatrixXd A(2,2);
MatrixXd B(2,1);
MatrixXd Qk(2,2);
MatrixXd H(1,2);
MatrixXd R(1,1);
MatrixXd I=MatrixXd::Identity(2,2);
Eigen::MatrixXd angle_1(1,1);
vector<MatrixXd> X_k;MatrixXd Xk=MatrixXd::Constant(2,1,0);
vector<MatrixXd> X_k_1;MatrixXd Xk_1=MatrixXd::Constant(2,1,0);
vector<MatrixXd> P_k;MatrixXd Pk=MatrixXd::Constant(2,2,0);
vector<MatrixXd> P_k_1;MatrixXd Pk_1=MatrixXd::Constant(2,2,0);
vector<MatrixXd> K_K;MatrixXd K=MatrixXd::Constant(2,2,0);
vector<MatrixXd> Y_K;MatrixXd Yk=MatrixXd::Constant(1,1,0);
class Kalman
{
public:
Kalman()
{
IMU imu;
//A<<1,imu.dt,0,1;
//B<<imu.dt,0;
Qk<<0.03,0,0,0.003;//QK的值暫定一個常量，后續(xù)需要求解角度方差和角速度偏差方差
H(0,0)=1;H(0,1)=0;
R<<3;//測量噪聲,暫時給一個定值
X_k.push_back(Xk);
X_k_1.push_back(Xk_1);
P_k.push_back(Pk);//此時刻的PK
P_k_1.push_back(Pk_1);//上一時刻的PK
K_K.push_back(K);
Y_K.push_back(Yk);
imu_sub=nd.subscribe<sensor_msgs::Imu>("imu", 100, &Kalman::IMUHandle,this);
position_pub=nd.advertise<sensor_msgs::Imu>("imu_position",1000);
}
void IMUHandle(const sensor_msgs::Imu::ConstPtr &imu_measure)
{
IMU Tx;
Tx.angle_v_x=imu_measure->angular_velocity.x;
Tx.angle_v_y=imu_measure->angular_velocity.y;
Tx.angle_v_z=imu_measure->angular_velocity.z;
Tx.acc_x=imu_measure->linear_acceleration.x;
Tx.acc_y=imu_measure->linear_acceleration.y;
Tx.acc_z=imu_measure->linear_acceleration.z;
double angle=-atan2(Tx.acc_x,sqrt(Tx.acc_y*Tx.acc_y+Tx.acc_z*Tx.acc_z));
double angle_1=angle*theta_PI;
double angle_v=Tx.angle_v_x;
// cout<<Tx.acc_x<<" "<<Tx.acc_y<<" "<<Tx.acc_z<<" "<<angle_1<<endl;
kalman_angle_x(angle,angle_v);
}
void kalman_angle_x(double angle,double angle_V)//angle是加速度計算出的角度 ,angle_V 角速度
{
IMU imu;
angle_1(0,0)=angle;
//卡爾曼第1個公式
Xk_1(0,0)=(angle_V-imu.bias)*(t-1)*0.04;
Xk_1(1,0)=imu.bias;
X_k_1[0](0,0)=Xk_1(0,0);
X_k_1[0](1,0)=Xk_1(1,0);
A<<1,imu.dt*t,0,1;
B<<imu.dt*t,0;
Xk=A*X_k_1[t-1]+B*angle_V;
X_k.push_back(Xk);
//卡爾曼第2個公式
Pk=A*P_k_1[t-1]*A.transpose()+Qk;
P_k.push_back(Pk);
//卡爾曼第3個公式
MatrixXd tmp(1,1);
tmp=H*P_k[t]*H.transpose()+R;
K=P_k[t]*H.transpose()*tmp.inverse();
K_K.push_back(K);
Yk=angle_1-H*X_k[t];
Y_K.push_back(Yk);
//卡爾曼第4個公式
Xk_1=X_k[t]+K_K[t]*Y_K[t];
X_k_1.push_back(Xk_1);
//卡爾曼第5個公式
Pk=(I-K_K[t]*H)*P_k[t];
P_k_1.push_back(Pk);
cout<<X_k[t](0,0)-angle<<endl;
// cout<<angle<<endl;
t++;
}
private:
ros::NodeHandle nd;
ros::Publisher position_pub;
ros::Subscriber imu_sub;
};
int main(int argc, char** argv)
{
ros::init(argc, argv,"kalman");
Kalman PublicIMU;
ros::spin();
return 0;
}
完整的代碼在我的gitee：
https://gitee.com/angry-potato/imu-kalman
效果展示在這里：
縱坐標(biāo)是觀測值和預(yù)測值之間的error，數(shù)據(jù)輸入全部都是來源于慣導(dǎo)，當(dāng)我快速抖動慣導(dǎo)之后，就有了上面的波浪部分，但是很快error又趨近于0。
什么？？？？？你說你看不懂C++？
我這里也有份MATLAB的代碼，可以參考一下，不過是網(wǎng)上的：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%程序功能： 使用擴展卡爾曼濾波器(EKF)估計平拋物體的運動
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
close all;
kx = .01; ky = .05; % 阻尼系數(shù)
g = 9.8; % 重力
t = 10; % 仿真時間
Ts = 0.1; % 采樣周期
len = fix(t/Ts); % 仿真步數(shù)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%（真實軌跡模擬）
dax = 1.5; day = 1.5; % 系統(tǒng)噪聲
X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 狀態(tài)模擬的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
x = x + vx*Ts;
vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
y = y + vy*Ts;
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
X(k,:) = [x, vx, y, vy];
end
figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 構(gòu)造量測量
mrad = 0.001;
dr = 10; dafa = 10*mrad; % 量測噪聲
for k=1:len
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);
Z(k,:) = [r, a];
end
figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ekf 濾波
Qk = diag([0; dax; 0; day])^2;
Rk = diag([dr; dafa])^2;
Xk = zeros(4,1);
Pk = 100*eye(4);
X_est = X;
for k=1:len
Ft = JacobianF(X(k,:), kx, ky, g);
Hk = JacobianH(X(k,:));
fX = fff(X(k,:), kx, ky, g, Ts);
hfX = hhh(fX, Ts);
[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
X_est(k,:) = Xk';
end
figure(1), plot(X_est(:,1),X_est(:,3), '+r')
xlabel('X'); ylabel('Y'); title('ekf simulation');
legend('real', 'measurement', 'ekf estimated');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function F = JacobianF(X, kx, ky, g) % 系統(tǒng)狀態(tài)雅可比函數(shù)
vx = X(2); vy = X(4);
F = zeros(4,4);
F(1,2) = 1;
F(2,2) = -2*kx*vx;
F(3,4) = 1;
F(4,4) = 2*ky*vy;
end
function H = JacobianH(X) % 量測雅可比函數(shù)
x = X(1); y = X(3);
H = zeros(2,4);
r = sqrt(x^2+y^2);
H(1,1) = 1/r; H(1,3) = 1/r;
xy2 = 1+(x/y)^2;
H(2,1) = 1/xy2*1/y; H(2,3) = 1/xy2*x*(-1/y^2);
end
function fX = fff(X, kx, ky, g, Ts) % 系統(tǒng)狀態(tài)非線性函數(shù)
x = X(1); vx = X(2); y = X(3); vy = X(4);
x1 = x + vx*Ts;
vx1 = vx + (-kx*vx^2)*Ts;
y1 = y + vy*Ts;
vy1 = vy + (ky*vy^2-g)*Ts;
fX = [x1; vx1; y1; vy1];
end
function hfX = hhh(fX, Ts) % 量測非線性函數(shù)
x = fX(1); y = fX(3);
r = sqrt(x^2+y^2);
a = atan(x/y);
hfX = [r; a];
end
function [Xk, Pk, Kk] = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 濾波函數(shù)
Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;
Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz^-1;
Xk = fXk_1 + Kk*Zk_hfX;
Pk = Pkk_1 - Kk*Pzz*Kk'
end以上。
下載1：OpenCV-Contrib擴展模塊中文版教程
在「小白學(xué)視覺」公眾號后臺回復(fù)：擴展模塊中文教程，即可下載全網(wǎng)第一份OpenCV擴展模塊教程中文版，涵蓋擴展模塊安裝、SFM算法、立體視覺、目標(biāo)跟蹤、生物視覺、超分辨率處理等二十多章內(nèi)容。
下載2：Python視覺實戰(zhàn)項目52講
在「小白學(xué)視覺」公眾號后臺回復(fù)：Python視覺實戰(zhàn)項目，即可下載包括圖像分割、口罩檢測、車道線檢測、車輛計數(shù)、添加眼線、車牌識別、字符識別、情緒檢測、文本內(nèi)容提取、面部識別等31個視覺實戰(zhàn)項目，助力快速學(xué)校計算機視覺。
下載3：OpenCV實戰(zhàn)項目20講
在「小白學(xué)視覺」公眾號后臺回復(fù)：OpenCV實戰(zhàn)項目20講，即可下載含有20個基于OpenCV實現(xiàn)20個實戰(zhàn)項目，實現(xiàn)OpenCV學(xué)習(xí)進階。
交流群
歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動駕駛、計算攝影、檢測、分割、識別、醫(yī)學(xué)影像、GAN、算法競賽等微信群（以后會逐漸細(xì)分），請掃描下面微信號加群，備注：”昵稱+學(xué)校/公司+研究方向“，例如：”張三 + 上海交大 + 視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據(jù)研究方向邀請進入相關(guān)微信群。請勿在群內(nèi)發(fā)送廣告，否則會請出群，謝謝理解~