實(shí)際工程項(xiàng)目中是怎么用卡爾曼濾波的?
共 15016字，需瀏覽 31分鐘
·
2024-04-19 10:20
點(diǎn)擊上方“小白學(xué)視覺(jué)”，選擇加"星標(biāo)"或“置頂”
重磅干貨，第一時(shí)間送達(dá)
編輯 | 汽車人
原文鏈接：https://www.zhihu.com/question/358334095
回答一
作者：李崇
鏈接：https://www.zhihu.com/question/358334095/answer/1160183841
兩大難題，一是運(yùn)動(dòng)學(xué)模型的建立，也就是預(yù)測(cè)方程。這個(gè)一方面可以通過(guò)比較細(xì)的系統(tǒng)辨識(shí)來(lái)做，這方面要結(jié)合具體的應(yīng)用背景，不一而足，也是普遍做的比較差的地方。
另一方面可用某個(gè)傳感器的輸出來(lái)做預(yù)測(cè)，比較典型的就是用加速度計(jì)做預(yù)測(cè)，再用陀螺做更新。
另一大問(wèn)題就是噪聲的統(tǒng)計(jì)特性咯。因?yàn)榻Ｕ`差不知道。觀測(cè)方程到還可以測(cè)一下ZRO算一下均值和方差。然后下一步就是大家喜聞樂(lè)見(jiàn)的調(diào)參的階段咯，據(jù)說(shuō)是群魔亂舞花樣百出。
我現(xiàn)實(shí)中只用過(guò)mems加速度計(jì)和陀螺的融合，還有就是無(wú)模型的跟蹤咯。也就是預(yù)測(cè)方程是0，純靠建模誤差的那個(gè)方差來(lái)做預(yù)測(cè)，效果咋說(shuō)呢，穩(wěn)態(tài)下看噪聲統(tǒng)計(jì)特性是好了些，但是動(dòng)態(tài)特性明顯滯后和變形了。應(yīng)該是我經(jīng)驗(yàn)不足吧
回答二
作者：雁集
鏈接：https://www.zhihu.com/question/358334095/answer/1009240518
先啰嗦一下在工程中卡爾曼濾波要解決的問(wèn)題是什么。
我們得到了2（或多個(gè)）個(gè)反饋同一件事的信息A和B，但不知道該相信哪個(gè)結(jié)果，所以簡(jiǎn)單的，將兩個(gè)信息相加除以2（加權(quán)平均），得到一個(gè)結(jié)果0.5A+0.5B。但這個(gè)方法過(guò)于粗暴，不能反映出水平。現(xiàn)在知道了其中一個(gè)信息可靠度更好，而另一個(gè)可靠度不好，就要更相信好的信息，如，現(xiàn)在給可信度好的B更大的權(quán)重，得到0.2A+0.8B，這個(gè)結(jié)果會(huì)好于第一個(gè)。但仍然覺(jué)得low，因?yàn)锳與B的參數(shù)是根據(jù)經(jīng)驗(yàn)設(shè)置的，并不知道是否是最合理的，是否是最優(yōu)化的。所以高級(jí)的，用優(yōu)化的方法，根據(jù)A與B的高斯分布特征，去融合A和B信息，這個(gè)就是卡爾曼。
所以，我們看到，要利用卡爾曼，需要不同的信息源（大于等于2個(gè)）。最常見(jiàn)的信息源是什么？就是我們根據(jù)系統(tǒng)模型或動(dòng)力學(xué)模型計(jì)算出來(lái)的結(jié)果，可以通過(guò)仿真的形式得到。第二個(gè)或第三個(gè)信息是什么？在飛行器姿態(tài)估計(jì)，導(dǎo)航定位，目標(biāo)跟蹤（幾個(gè)問(wèn)題的本質(zhì)一樣，都是狀態(tài)估計(jì)）的問(wèn)題中，這個(gè)信息一般來(lái)自慣導(dǎo)或GPS或雷達(dá)等。有了2個(gè)信息數(shù)據(jù)，就可以根據(jù)Kalman的公式進(jìn)行融合。下面舉一個(gè)機(jī)器人運(yùn)動(dòng)估計(jì)的例子，來(lái)自《Probabilistic Robotics》。
首先假設(shè)你的機(jī)器人長(zhǎng)這樣（一個(gè)圓，比較簡(jiǎn)單），在二維笛卡爾坐標(biāo)系中運(yùn)動(dòng)。
根據(jù)這個(gè)圖，可以得到它的運(yùn)動(dòng)學(xué)模型
得到以上模型還不能直接用，這個(gè)模型有變量的乘積和三角函數(shù)，是個(gè)非線性模型，但要利用卡爾曼公式，需要滿足EKF（實(shí)際工程應(yīng)用中，我還沒(méi)見(jiàn)過(guò)直接用KF的）的線性化條件。所以要將非線性的模型線性化。
到這一步，得到的Gt可以直接代入EKF公式去更新協(xié)方差矩陣了，經(jīng)過(guò)推算會(huì)得到增益矩陣K。EKF公式包括預(yù)測(cè)與更新，這里就不啰嗦了。
同時(shí)，用狀態(tài)的迭代形式，可以得到初步推算的位置。
最后，EKF公式中有一項(xiàng)
其中兩個(gè)z，代表了2個(gè)信息，比如其中一個(gè)是通過(guò)動(dòng)力學(xué)得到的機(jī)器人位置，另一個(gè)通過(guò)雷達(dá)得到的機(jī)器人位置。
通過(guò)以上的計(jì)算，會(huì)得到一個(gè)靠譜的結(jié)果。
Kalman的其它公式都是用優(yōu)化理論去解算高斯函數(shù)的，在此不啰嗦了。
回答三
作者：虛函數(shù)機(jī)器人
鏈接：https://www.zhihu.com/question/358334095/answer/2396600132
關(guān)于卡爾曼原理的講解知乎上有太多了，而且理解的都比我好，在這里我就不班門弄斧了。我實(shí)現(xiàn)了一個(gè)融合慣導(dǎo)的加速度計(jì)和陀螺儀的簡(jiǎn)易卡爾曼濾波器，代碼寫的十分簡(jiǎn)單，保證初學(xué)者一看就懂，對(duì)那些復(fù)現(xiàn)算法有困難的同學(xué)會(huì)起到一定幫助。話不多說(shuō)，上干貨： 會(huì)用到的卡爾曼腦仁疼公式如下：
加速度計(jì)數(shù)學(xué)模型滿足：
代碼融合如下，代碼風(fēng)格不好，各位別學(xué)我。（寫這個(gè)代碼的時(shí)候我還未成年）：
#include "imu.h"
#include <iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<string>
#include<vector>
#include <cmath>
#include <limits>//用于生成隨機(jī)分布數(shù)列
#include <stdlib.h>
#include <random>
#include <fstream>
#include <cstdlib> // 標(biāo)準(zhǔn)庫(kù)
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的值暫定一個(gè)常量，后續(xù)需要求解角度方差和角速度偏差方差
H(0,0)=1;H(0,1)=0;
R<<3;//測(cè)量噪聲,暫時(shí)給一個(gè)定值
X_k.push_back(Xk);
X_k_1.push_back(Xk_1);
P_k.push_back(Pk);//此時(shí)刻的PK
P_k_1.push_back(Pk_1);//上一時(shí)刻的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是加速度計(jì)算出的角度 ,angle_V 角速度
{
IMU imu;
angle_1(0,0)=angle;
//卡爾曼第1個(gè)公式
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個(gè)公式
Pk=A*P_k_1[t-1]*A.transpose()+Qk;
P_k.push_back(Pk);
//卡爾曼第3個(gè)公式
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個(gè)公式
Xk_1=X_k[t]+K_K[t]*Y_K[t];
X_k_1.push_back(Xk_1);
//卡爾曼第5個(gè)公式
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)是觀測(cè)值和預(yù)測(cè)值之間的error，數(shù)據(jù)輸入全部都是來(lái)源于慣導(dǎo)，當(dāng)我快速抖動(dòng)慣導(dǎo)之后，就有了上面的波浪部分，但是很快error又趨近于0。
什么？？？？？你說(shuō)你看不懂C++？
我這里也有份MATLAB的代碼，可以參考一下，不過(guò)是網(wǎng)上的：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%程序功能： 使用擴(kuò)展卡爾曼濾波器(EKF)估計(jì)平拋物體的運(yùn)動(dòng)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
close all;
kx = .01; ky = .05; % 阻尼系數(shù)
g = 9.8; % 重力
t = 10; % 仿真時(shí)間
Ts = 0.1; % 采樣周期
len = fix(t/Ts); % 仿真步數(shù)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%（真實(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)造量測(cè)量
mrad = 0.001;
dr = 10; dafa = 10*mrad; % 量測(cè)噪聲
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) % 量測(cè)雅可比函數(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) % 量測(cè)非線性函數(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擴(kuò)展模塊中文版教程
在「小白學(xué)視覺(jué)」公眾號(hào)后臺(tái)回復(fù)：擴(kuò)展模塊中文教程，即可下載全網(wǎng)第一份OpenCV擴(kuò)展模塊教程中文版，涵蓋擴(kuò)展模塊安裝、SFM算法、立體視覺(jué)、目標(biāo)跟蹤、生物視覺(jué)、超分辨率處理等二十多章內(nèi)容。
下載2：Python視覺(jué)實(shí)戰(zhàn)項(xiàng)目52講
在「小白學(xué)視覺(jué)」公眾號(hào)后臺(tái)回復(fù)：Python視覺(jué)實(shí)戰(zhàn)項(xiàng)目，即可下載包括圖像分割、口罩檢測(cè)、車道線檢測(cè)、車輛計(jì)數(shù)、添加眼線、車牌識(shí)別、字符識(shí)別、情緒檢測(cè)、文本內(nèi)容提取、面部識(shí)別等31個(gè)視覺(jué)實(shí)戰(zhàn)項(xiàng)目，助力快速學(xué)校計(jì)算機(jī)視覺(jué)。
下載3：OpenCV實(shí)戰(zhàn)項(xiàng)目20講
在「小白學(xué)視覺(jué)」公眾號(hào)后臺(tái)回復(fù)：OpenCV實(shí)戰(zhàn)項(xiàng)目20講，即可下載含有20個(gè)基于OpenCV實(shí)現(xiàn)20個(gè)實(shí)戰(zhàn)項(xiàng)目，實(shí)現(xiàn)OpenCV學(xué)習(xí)進(jìn)階。
交流群
歡迎加入公眾號(hào)讀者群一起和同行交流，目前有SLAM、三維視覺(jué)、傳感器、自動(dòng)駕駛、計(jì)算攝影、檢測(cè)、分割、識(shí)別、醫(yī)學(xué)影像、GAN、算法競(jìng)賽等微信群（以后會(huì)逐漸細(xì)分），請(qǐng)掃描下面微信號(hào)加群，備注：”昵稱+學(xué)校/公司+研究方向“，例如：”張三 + 上海交大 + 視覺(jué)SLAM“。請(qǐng)按照格式備注，否則不予通過(guò)。添加成功后會(huì)根據(jù)研究方向邀請(qǐng)進(jìn)入相關(guān)微信群。請(qǐng)勿在群內(nèi)發(fā)送廣告，否則會(huì)請(qǐng)出群，謝謝理解~