# [Brushless] LAN1902 - 线性卡尔曼滤波器 ## 0. 前言

BLDC也就是“直流无刷电机”，和PMSM（永磁同步电机）并列是目前应用最广泛的电机类型，在高速，高可靠性，小体积，高效率，少维护等要求下是首选。和有刷电机依靠机械结构进行电-磁控制不同，它要求控制器去开关MOSFET来产生特定的磁场，以维持马达转动。但是如果在不合适的时间打开关闭MOSFET，会导致马达产生相反方向的驱动力或者内部各绕组所产生的磁场的作用效果相互抵消，大大降低效率，乃至让马达停转。因此如何控制这个导通的时间点就成为了关键，为了知道应该在什么时候控制开关动作，需要知道转子当前到达了什么位置。

• 电磁传感器：通过电机内磁场变化导致的感应电场（类似变压器的原理）或者转子引起谐振频率的改变测量位置。缺点是分辨率差，成本高
• 磁敏传感器：通过霍尔效应测量磁场，目前使用最广泛
• 光电传感器：转子上安装遮光板，通过测量光线变化得出转子位置，缺点是对灰尘和震动很敏感 ...

## 3. 卡尔曼滤波器的原理 ## 4. MATLAB例

### A. 一维线性卡尔曼滤波算法

% LINEAR KALMAN FILTER ALGORITHM IMPLEMENTATION
% WRITTEN BY:   Andy Li @ Lithcore.cn
% DATE:         Aug-06-2019
% FUNCTION:
clear all;
close all;

n_iter = 500;
Q = 0.1;
R = 100;
A0 = 0.1;
B0 = 2;
size = [n_iter, 1];
f = zeros(size);
xh_plot = zeros(size);
x_plot_delayed = zeros(size);
z_plot = zeros(size);
error_measure = zeros(size);
error_kalman = zeros(size);

f(1) = B0;
z = f(1);
x = f(1);
xh = B0;
Ph = 0.01;

z_plot(1) = z;
xh_plot(1) = xh;
x_plot_delayed(1) = xh;

for k = 2:n_iter
f(k) = A0*k+B0;				% The Input Function of Interest
z = f(k)+sqrt(R)*randn(1);	% Measured Value

%% PREDICT %%
x = xh + A0;		% Refined System Model, if remove A0,
P = Ph + Q;			% there'll be an stationary error

%% UPDATE %%
K = P/(P+R);
xh = x + K*(z-x);
Ph = (1-K)*P;

z_plot(k) = z;
xh_plot(k) = xh;
x_plot_delayed(k) = xh;
end
%% ERROR PLOT %%
for k=1:n_iter
error_measure(k) = abs(z_plot(k) - f(k));
error_kalman(k)  = abs(xh_plot(k) - f(k));
end

figure(1);
hold on;
plot(f,'g-');
plot(xh_plot, 'b-');
plot(z_plot, 'k+');
legend('Actual', 'Estimation', 'Measured');

figure(2);
hold on;
plot(error_measure,'r-');
plot(error_kalman, 'b-');
legend('Measured', 'Filtered'); ### B. 二维运动路径预测例 xy合并起来写成: clear all;
close all;

n_smpl = 500;

x0 = 1000;
y0 = -800;
vx = 0;
vy = 20;
ax = 0;
ay = 0;
dSx = 0;
dSy = 0;

T = 2;

F_d = [1 T;
0 1];
F = [F_d        zeros(2,2);					% 系统模型
zeros(2,2) F_d       ];

sigma_a = 0.1;
Q_d= [T^2     0.5*T^3 ;
0.5*T^3 0.25*T^4] .* sigma_a^2;
Q = [Q_d        zeros(2,2);					% 过程噪声协方差矩阵
zeros(2,2) Q_d       ];

sigma = 10;
sigma_x = sigma;
sigma_y = sigma;
r = [sigma_x^2; sigma_y^2];
R = diag(r);								% 观测噪声协方差
H = [1  0  0  0;							% 观测模型
0  0  1  0];

% Initialize Variables
Z_k = [x0; y0];
Xhat_prev = [x0; vx; y0; vy];	% 状态初始值
Phat_prev = Q;

Xhat_plot = zeros(4,n_smpl);
X_plot = zeros(4, n_smpl);
Z_plot = zeros(2,n_smpl);
S_plot = zeros(2,n_smpl);
error_plot_kf = zeros(1, n_smpl);
error_plot_meas = zeros(1, n_smpl);

for i=1:n_smpl
if i>50 && i<=75
ax = 0.2;
end
if i>75 && i <= 100
ax = -0.2;
end
if i>100 && i<=280
ax = 0;
vx = 10*sin(2*pi*(i-100)/360);
vy = 10*cos(2*pi*(i-100)/360);
end
if i>280 && i<300
ay = -0.1;
end
if(i>300)&&(i<480)
ay = 0;
vx = 10*sin(0.5*2*pi*(i-300)/360+pi);
vy = 10*cos(0.5*2*pi*(i-300)/360+pi);
end
if i>480
ax = -1;
end

dSx = dSx + vx * T + 0.5*(ax*T^2);					% 产生实际值
dSy = dSy + vy * T + 0.5*(ay*T^2);					%
vx = vx + ax * T;									%
vy = vy + ay * T;									%
S_k = [x0 + dSx; y0 + dSy];							% /

Z_k = S_k + r.*randn(2,1);							% 引入观测噪声

% 预测
X = F * Xhat_prev;									% 预测状态值
P = F * Phat_prev * F'+ Q;							% 预测协方差矩阵
% 更新
K = P * H'/(H * P * H' + R);						% 计算卡尔曼增益
Xhat = X + K * (Z_k - H * X);						% 估计状态值
Phat = (eye(4) - K * H) * P;						% 估计协方差矩阵

X_plot(:,i) = X;									% 保存作图数据
Xhat_plot(:,i) = Xhat;								%
Z_plot(:,i) = Z_k;									%
S_plot(:,i) = S_k;									%
error_plot_kf(i) = ...								%
sqrt((Xhat(1)-S_k(1))^2+(Xhat(3)-S_k(2))^2);	%
error_plot_meas(i) = ...							%
sqrt((Z_k(1)-S_k(1))^2+(Z_k(2)-S_k(2))^2);		% /

Phat_prev = Phat;
Xhat_prev = Xhat;

end

figure(1);
hold on;
plot(Z_plot(1,:), Z_plot(2,:), 'k+');
plot(S_plot(1,:), S_plot(2,:), 'r-');
plot(Xhat_plot(1,:), Xhat_plot(3,:), 'b-');
xlabel('X');
ylabel('Y');
legend('Measured','Actual','Filtered');

figure(2);
hold on;
plot(error_plot_kf);
plot(error_plot_meas);
xlabel('T');
ylabel('Distance');
legend('Xhat', 'Z');

## 6. 参考文献

. Microchip. AN1083 使用反电动势滤波进行无传感器BLDC 控制

. 谭建成, 邵晓强. 永磁无刷直流电机技术

. Kalmanfilter.net

. Bzarg. How a Kalman filter works, in pictures

. Paul Kettle, Aengus Murray, Finbarr Moynihan. Sensorless Control of a Brushless DC Motor Using an Extended Kalman Estimator. PCIM'98 Intelligent Motion, 1998

. aerith7’s blog. apollo11号のソースコードを読みつつ. 2016

. Leonard A. McGee, Stanley F. Schmidt. Discovery of the Kalman Filter as a Practical Tool for Aerospace and Industry. NASA Technical Memorandum 86847, NASA Archive. Nov. 1985

. R. E. KALMAN. A New Approach to Linear Filtering and Prediction Problems. Trans. ASME–Journal of Basic Engineering, 82 (Series D): 35-45. 1960

. 半闲居士. SLAM中的EKF，UKF，PF原理简介

. fishmarch. 概率机器人——扩展卡尔曼滤波、无迹卡尔曼滤波. 北京航空航天大学 机器人研究所

. Thrun S. Probabilistic robotics[M]. MIT Press, 2006.

. 虞波, 周翟和, 赵庆涛, 胡佳佳. 一种带遗忘因子的自适应卡尔曼滤波器及其在移动机器人中的应用. 电气与自动化. Aug 2016.

. Wikipedia. Alpha beta filter

## One thought on “[Brushless] LAN1902 - 线性卡尔曼滤波器”

1. AndyLithia says:

草生，这个LaTex插件如果开了MathJax在一些浏览器里干脆就一片空白加载不出来，于是现在我把全站的公式显示都改成图片模式了，不知道会不会给host带来比较大的负担……
此外Crayon对MATLAB的着色处理的实在是有点问题，该考虑找别的代码着色插件了，

This site uses Akismet to reduce spam. Learn how your comment data is processed.
{"ft_lang_label":"__label__zh","ft_lang_prob":0.92472166,"math_prob":0.9995974,"size":12609,"snap":"2020-45-2020-50","text_gpt3_token_len":10901,"char_repetition_ratio":0.06545022,"word_repetition_ratio":0.0,"special_character_ratio":0.2389563,"punctuation_ratio":0.18096906,"nsfw_num_words":0,"has_unicode_error":false,"math_prob_llama3":0.9998298,"pos_list":[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30],"im_url_duplicate_count":[null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,1,null,null,null],"WARC_HEADER":"{\"WARC-Type\":\"response\",\"WARC-Date\":\"2020-10-20T02:56:45Z\",\"WARC-Record-ID\":\"<urn:uuid:ff5c056a-f920-43d9-911b-15c6f13364b6>\",\"Content-Length\":\"117957\",\"Content-Type\":\"application/http; msgtype=response\",\"WARC-Warcinfo-ID\":\"<urn:uuid:6d065d60-8d01-4e6b-bf77-acf9974abdcd>\",\"WARC-Concurrent-To\":\"<urn:uuid:471900dd-12b4-4f71-953a-86c99c7f754e>\",\"WARC-IP-Address\":\"103.144.88.135\",\"WARC-Target-URI\":\"https://lithcore.cn/2532\",\"WARC-Payload-Digest\":\"sha1:NFBI4IP4BFBS63366W3PFGAI5SI3CKHK\",\"WARC-Block-Digest\":\"sha1:ICVK56YLNJ62PJEFUWVTEPRUVLPHFXPZ\",\"WARC-Identified-Payload-Type\":\"text/html\",\"warc_filename\":\"/cc_download/warc_2020/CC-MAIN-2020-45/CC-MAIN-2020-45_segments_1603107869785.9_warc_CC-MAIN-20201020021700-20201020051700-00568.warc.gz\"}"}