基于Matlab实现扩展卡尔曼滤波(EKF)源码+超详细注释+项目说明.zip_卡尔曼滤波matlab代码,扩展卡尔曼滤波matlab资源-CSDN下载

共17个文件

m：6个

mat：6个

jpg：3个

版权申诉

毕业设计

项目源码

课程大作业

Matlab

5星 · 超过95%的资源 26 浏览量 2023-09-25 11:26:21 上传评论 7 收藏 551KB ZIP 举报

资源推荐

资源详情

资源评论

收起资源包目录

基于Matlab实现扩展卡尔曼滤波(EKF)源码+超详细注释+项目说明.zip （17个子文件）

lab_ode_rk4_explicit_x_withnoise.m 409B

t.mat 2KB

compare_Jacobian.m 1KB

项目说明.md 5KB

lab_ode_rk4_explicit.m 398B

X_est_analy_Fk.mat 24KB

main.m 10KB

P_nrm_digital_Fk.mat 6KB

2019 РЕАЛИЗАЦИЯ РАСШИРЕННОГО ФИЛЬТРА КАЛМАНА.pdf 331KB

P_nrm_analy_Fk.mat 6KB

X_rk4.mat 21KB

lab_ode_rk4_explicit_x.m 400B

imgs

2.jpg 48KB

1.jpg 80KB

3.jpg 37KB

lab_ode_rk4_explicit_p.m 351B

X_est_digital_Fk.mat 24KB

Политехнический молодежный журнал. 2019. № 08 1

УДК 519.254 DOI: 10.18698/2541-8009-2019-08-513

РЕАЛИЗАЦИЯ РАСШИРЕННОГО ФИЛЬТРА КАЛМАНА

ДЛЯ НЕЛИНЕЙНОЙ МОДЕЛИ ДВУХЗВЕННОГО МАЯТНИКА

Г.В. Лысухо

А.Л. Масленников

lyysukhogv@student.bmstu.ru

SPIN-код: 8276-4150

amas@bmstu.ru

SPIN-код: 7087-6303

МГТУ им. Н.Э. Баумана, Москва, Российская Федерация

Аннотация Ключевые слова

Для оценки вектора состояния динамических систем

применяют различные виды калмановской фильтра-

ции. Для нелинейных систем можно использовать

расширенный или сигма-точечный фильтр Калмана,

которые имеют ряд сложностей при практической

реализации. Наличие этих сложностей приводит к

повышению требований к вычислительным ресурсам,

а также к возможной потере алгоритмом вычисли-

тельной устойчивости и увеличению ошибки оценки

вектора состояния. В статье рассмотрено примене-

ние гибридной версии расширенного фильтра Калмана

первого порядка, которая позволяет устранить ряд

сложностей при практической реализации. В каче-

стве математической модели выбрана модель двух-

звенного маятника. При используемой реализации

фильтра Калмана гарантируется симметричность

априорной ковариационной матрицы, а также при-

меняются численные методы вычисления матрицы

первых частных производных и QR-разложения при

вычислении матрицы усиления Калмана.

Расширенный фильтр Калмана,

ибридный фильтр Калмана,

нелинейные системы, двухзвен-

ный маятник, метод конечных

разностей, QR-разложение, оцен-

ка вектора состояния, теория

оценивания

Поступила в редакцию 02.07.2019

Введение. В системах автоматического управления зачастую требуется оцени-

вать вектор состояния (или просто состояние системы), например, в задачах

синтеза дискретных систем или задачах фильтрации навигационной информа-

ции [1, 2]. На практике численную оценку вектора состояния при наличии

входных и измерительных шумов можно получить с помощью различных

фильтров Калмана (линейного, расширенного или сигма-точечного) [3−6]. При

неизвестных параметрах входных и измерительных шумов или их нестационар-

ности, как правило, применяют адаптивные фильтры Калмана, которые не яв-

ляются отдельным видом калмановских фильтров, а расширяют функционал

линейного, расширенного или сигма-точечного фильтров Калмана [7−9].

Для линейных систем можно использовать линейные фильтры Калмана, а для

нелинейных следует применять расширенные или сигма-точечные фильтры Кал-

мана, практическая реализация которых сопряжена с рядом сложностей. Во-

本文使用一阶混合型卡尔曼滤波

研究双连杆模型

数值方法计算一阶偏导矩阵——雅可比矩阵

Г.В. Лысухо, А.Л. Масленников

2 Политехнический молодежный журнал. 2019. № 08

первых, вычислительная устойчивость фильтров Калмана сильно зависит от точ-

ности задания начальных условий и знания ковариационных матриц шумов, кото-

рые априори неизвестны [4, 5]. Во-вторых, для расширенного фильтра Калмана

первого порядка необходимо предварительно получить в аналитическом виде мат-

рицу первых частных производных — матрицу Якоби, что для сложных динамиче-

ских систем может быть крайне затруднительным. Помимо этого для вычисления

матрицы Якоби (как и ряда других векторно-матричных операций) на каждой ите-

рации фильтра Калмана требуется высокая производительность контроллера, что

приводит к необходимости оптимизации численных методов расчета.

В работе рассмотрено применение расширенного фильтра Калмана первого

порядка к оценке вектора состояния нелинейной модели непрерывной системы

двухзвенного маятника. В статье описана нелинейная математическая модель

рассматриваемой физической системы, приведен обобщенный алгоритм рас-

ширенного фильтра Калмана первого порядка, отмечен ряд сложностей его

практической реализации и указаны пути их устранения. В частности, для оп-

тимизации вычисления оценки вектора состояния применены численные под-

ходы к определению матрицы Якоби (не требует нахождения первых производ-

ных в аналитическом виде) и QR-разложения при вычислении апостериорной

оценки вектора состояния. В заключении приведены результаты оценки вектора

состояния с помощью реализованного фильтра Калмана.

Математическая модель двухзвенного ма-

ятника. Двухзвенный маятник — непрерывная

динамическая система, состоящая из двух зве-

ньев, кинематическая схема которой представ-

лена на рис. 1. Двухзвенный маятник, в том

числе и перевернутый (или обратный), очень

часто применяют в качестве упрощенной моде-

ли различных систем, например, верхней части

туловища человека [10−12].

Математическая модель невозмущенного

двухзвенного маятника состоит из двух дифферен-

циальных уравнений, имеющих следующий вид:

211 12 12 21 12 2

2 2 1 2 212 2 1 2 1 2 1 1 1

2 22

1 2 1 21 1 2

212 1 1 2 22 2 2 2

cos( )sin( ) cos( )sin

cos( ) sin( ) ( ) sin

;

( ) cos ( )

sin( ) sin

m l m gl

m l l m m gl

m m l ml

mll ml g



− α α −α α −α + α −α α +



+ lα α−α − α α−α − + α−lα





+ − α −α

α α−α − α−lα−

α=



 



212 1 2 1

cos( )

mll









α −α α







где

— угол поворота;



— угловая скорость;



— угловое ускорение; g —

ускорение свободного падения;

— длина;

— масса материальной точки;

Рис. 1. Кинематическая схема

двухзвенного маятника

对于非线性系统应用EKF和UKF遇到的困难：

卡尔曼滤波器的计算稳定性在很大程度上取决于初始条件的准确性和噪声协方差矩阵，而这些都是事先不知

道的[4, 5]。其次，对于一阶扩展卡尔曼滤波来说，有必要以解析形式获得一阶偏导矩阵，即雅可比矩阵，这

对于复杂的动力系统可能是非常困难的。此外，为了在卡尔曼滤波器的每次迭代中计算出雅可比矩阵（以及

其他一些矢量矩阵），需要高的控制器性能。导致了优化数值计算方法的必要性

数值方法(非解析法)求雅可比

矩阵

QR分解计算后验误差协方差

矩阵

Реализация расширенного фильтра Калмана для нелинейной модели двухзвенного …

Политехнический молодежный журнал. 2019. № 08 3

— коэффициент диссипации, а индексы 1 и 2 указывают на порядковый но-

мер соответствующего звена.

Для решения задачи численного моделирования системы и оценки вектора

состояния описание системы необходимо получить в форме Коши с учетом

наличия внешних управляющих моментов

. Для этого выполняют заме-

ну переменных:









Здесь

— вектор состояния системы, а динамика системы описывается с

помощью четырех дифференциальных уравнений первого порядка

x f(x,u, ),t=



где правые части имеют следующий поэлементный вид:

21212 2 114 241

212 4 2 1 2 1 3 1 2

2 22

1 2 1 21 1

212 2 2 22 4 2 4 212 1 2

;

()

;

()

;

ml x BB mglBB x B

m l l x B m m gl B x

m m l ml B

mllx B ml gB x mllBx



− + +l −



− − + −l



+−

− −l −



с учетом следующих обозначений:

1 13

2 13

cos( );

sin( );

sin ;

sin .

B xx

=−

Моделирование системы можно реализовать явным методом Рунге — Кут-

ты четвертого порядка, в котором после инициализации начальных условий

x x( )t=

, которые в данном случае можно принять нулевыми, на каждой k-й

итерации для момента времени

(при

1kk

ht t

−

=−

) вычисляют значения

1234

,,,KKKK

, а затем и решение

(x , );

Kf t=

为了解决系统的数值建模问题，并估计出状态向量，

系统的描述必须以Cauchy形式得到，同时考虑到存在

外部控制时刻u1和u2。要做到这一点，就要替换掉这

些变量。

系统状态方程

四阶显式龙格库塔法求解微分方程组

Г.В. Лысухо, А.Л. Масленников

4 Политехнический молодежный журнал. 2019. № 08

(x 0,5 , 0,5 );

(x , );

K f hK t h

=+ +

1 1 2 34

x x ( 2 2 ).

K K KK

=+ + + +

Модель измерений рассматриваемой системы сформирована для случая

измерения двух углов — углов поворота каждого из звеньев, и в общем виде за-

писывается следующим образом:

y x.

Результат моделирования системы явным методом Рунге — Кутты четвер-

того порядка при ступенчатом входном воздействии

равны единице с

момента времени

1t =

с) без наличия входных и измерительных шумов пред-

ставлен на рис. 2.

Рис. 2. Результат моделирования нелинейной модели двухзвенного маятника

при ступенчатом входном воздействии:

а — углы поворота звеньев; б — угловые скорости

Добавление входных и измерительных шумов реализуется следующим об-

разом. В правых частях дифференциальных уравнений присутствует

, кото-

рое можно представить в виде

u r w,

kk k

где

— полезный входной сигнал;

— входной шум. Добавление измери-

тельного шума

реализуется использованием следующей модели измерений:

yxν.

k kk

C=+

系统的输出是两个旋转角

评论收藏

内容反馈

版权申诉

普通网友

2025-03-27

资源中能够借鉴的内容很多，值得学习的地方也很多，大家一起进步！
QQQQ1637

2024-05-25

怎么能有这么好的资源！只能用感激涕零来形容TAT...

onnx
上传者
2025-03-20

认可支持
m0_75230343

2024-05-02

资源是宝藏资源，实用也是真的实用，感谢大佬分享~

onnx
上传者
2025-03-20

欢迎学习
qq_57969254

2024-02-08

资源内容总结地很全面，值得借鉴，对我来说很有用，解决了我的燃眉之急。

onnx
上传者
2025-03-20

xiexie