MPC as a Copilot: A Predictive Filter Framework with Safety and Stability Guarantees

M P C A S A C O P I L OT : A P R E D I C T I V E F I L T E R F R A M E W O R K W I T H S A F E T Y A N D S T A B I L I T Y G UA R A N T E E S Y unda Y an Department of Computer Science Univ ersity College London, London WC1E 6BT , UK yunda.yan@ucl.ac.uk Chenxi T ao School of Automation Southeast Univ ersity , Nanjing 210096, China chenxi.tao@seu.edu.cn Jinya Su School of Automation Southeast Univ ersity , Nanjing 210096, China sucas@seu.edu.cn Cunjia Liu Department of Aeronautical and Automotiv e Engineering Loughborough Univ ersity , Leicestershire LE11 3TU, UK c.liu5@lboro.ac.uk Shihua Li School of Automation Southeast Univ ersity , Nanjing 210096, China lsh@seu.edu.cn March 31, 2026 A B S T R AC T Ensuring both safety and stability remains a fundamental challenge in learning-based control, where goal-oriented policies often neglect system constraints and closed-loop state con ver gence. T o ad- dress this limitation, this paper introduces the Predictive Safety–Stability F ilter (PS 2 F), a uniﬁed predictiv e ﬁlter framew ork that guarantees constraint satisfaction and asymptotic stability within a single architect ure. The PS 2 F framework comprises two cascaded optimal control problems: a nom- inal model predictive control (MPC) layer that serves solely as a copilot , implicitly deﬁning a L ya- punov function and generating safety- and stability-certiﬁed predicted trajectories, and a secondary ﬁltering layer that adjusts external command to remain within a pro vably safe and stable region. This cascaded structure enables PS 2 F to inherit the theoretical guarantees of nominal MPC while accom- modating goal-oriented external commands. Rigorous analysis establishes recursive feasibility and asymptotic stability of the closed-loop system without introducing additional conserv atism beyond that associated with the nominal MPC. Furthermore, a time-v arying parameterisation allo ws PS 2 F to transition smoothly between safety-prioritised and stability-oriented operation modes, providing a principled mechanism for balancing exploration and exploitation. The effecti veness of the proposed framew ork is demonstrated through comparative numerical e xperiments. Keyw ords Predicti ve safety–stability ﬁlter; Model predictiv e control; Safety and stability; Learning-based control 1 Introduction Recent adv ances in machine learning and high-ﬁdelity simulators, coupled with increases in computational po wer and hardware efﬁcienc y , ha ve sparked growing interest in learning-based and data-dri ven control methods [1]. Although these methods hav e demonstrated impressi ve performance in various real-w orld applications, such as aerial and ground vehicles [2, 3], their theoretical foundations remain relativ ely underde veloped, with only a limited number of results providing rigorous guarantees [4, 5]. This lack of formal guarantees presents signiﬁcant risks in safety-critical domains such as robotics and automated dri ving, where violations of safety constraints can lead to catastrophic consequences [6, 7]. T o mitigate this research gap, ﬁlter-like frame works hav e emerged as a promising paradigm, serving as a safety layer that monitors and, whene ver necessary , adjusts control inputs to ensure constraint satisfaction [8]. Most e xisting MPC as a Copilot: A Predicti ve Filter Frame work with Safety and Stability Guarantees approaches rely on the construction of a control in variant set, either explicitly or implicitly , that guarantees safe system operation when subject to learning-based control inputs. For instance, the control barrier function (CBF) framew ork [9] employs a barrier function to characterise a safe set and enforces its in variance through a differential inequality . By contrast, the predicti ve safety ﬁlter (PSF) [10] lev erages a constrained optimisation problem inspired by model predicti ve control (MPC), typically incorporating a terminal constraint to guarantee recursiv e feasibility and, thereby , safety . Both approaches ha ve demonstrated remarkable success in ensuring safety for robotic systems [11, 12]. Howe ver , safety alone may not be sufﬁcient. Persistent oscillations within the constraints can still degrade performance and undermine reliability . Embedding asymptotic stability into the safety framework is therefore crucial to ensure both constraint satisfaction and con vergence, yet only a few attempts hav e been made in this direction. W ithin the CBF framew ork, a control L yapunov function (CLF) can be incorporated into the optimisation problem to enforce stability; howe ver , this often leads to infeasibility due to the absence of a uniﬁed CLF design [9]. Introducing a slack variable in the CLF constraint can alleviate this issue, but stability still cannot be guaranteed because the slack v ariable cannot, in general, be forced to remain zero at all times [13]. In the absence of an embedded CLF , the e xternal control law is generally required to stabilise to ensure both asymptotic stability and constraint satisfaction [14]. In [15], a uni versal barrier function is proposed to guarantee safety and stability simultaneously . Howe ver , this approach requires the nominal controller to be expressed in an integral form and is therefore not directly applicable to general learning-based or data-driv en control settings. W ithin the PSF framework, stability results are ev en fewer . In [16, 17], an additional stability constraint can be embedded into the original optimisation problem to ensure asymptotic stability; howe ver , this modiﬁcation may compromise the recursive feasibility property of the original formulation. Guaranteeing both safety and asymptotic stability without excessiv e conserv atism is therefore already challenging, and an e ven more theoretically signiﬁcant and practically meaningful question is whether the user can manually determine when the system should be gin to stabilise. This capability is particularly valuable in exploration–exploitation scenarios in robotics [18], where safety must be maintained during the e xploration or learning phase while asymptotic stability is enforced once reliable system kno wledge becomes av ailable. Shared autonomy [19] and human–robot interaction [20] ex emplify such settings: user-generated commands may be suboptimal or ev en unsafe, requiring the controller to grant operators sufﬁcient freedom to express intent (safe exploration) while ensuring that all ex ecuted actions remain safe and ultimately driv e the system back to a stable operating regime (stable exploitation). Motiv ated by the aforementioned research challenges and gaps, we propose the Pr edictive Safety–Stability F ilter (PS 2 F), a uniﬁed predicti ve control framew ork consisting of two optimal control problems (OCPs) arranged in a cascaded structure. The ﬁrst OCP is a nominal MPC problem whose value function implicitly serves as a L yapunov function for the nonlinear constrained system. Its optimal input and state trajectories represent the ideal, theoretically certiﬁed solutions that guarantee both safety and stability; howev er , these inputs are not applied directly to the system, but instead function as a copilot . The external controller acts as a pilot , generating goal-oriented commands that pursue high-level tasks or learning objectiv es. The second OCP then reconciles these two objectives by computing a control input that remains as close as possible to the external command while ensuring it lies within a provably safe and stable domain, as illustrated in Fig. 1. This layered structure enables PS 2 F to inherit the recursiv e feasibility and stability guarantees of nominal MPC while preserving the ﬂexibility needed to accommodate arbitrary external commands. Importantly , the second OCP is constructed so as not to introduce additional conservatism: feasibility is preserved as long as the initial state lies within the nominal MPC’ s region of attraction. Furthermore, by appropriately designing time-varying parameters, PS 2 F can prioritise safety during an initial phase and then deliberately transition into a stability-enforcing mode at a user-speciﬁed time. The ability of PS 2 F to mediate between arbitrary external commands and safety–stability–certiﬁed feedback makes it particularly well suited for human-in-the-loop applications such as shared autonomy and human–robot interaction, where operators must retain freedom of intent while the system remains safe and ultimately con vergent. The remainder of this paper is organised as follows. Section 2 revisits the nominal MPC formulation and the conditions required to ensure recursiv e feasibility and asymptotic stability . Section 3 presents the proposed PS 2 F framew ork and provides a detailed theoretical analysis. Section 4 demonstrates the effecti veness of the proposed method through comparativ e numerical studies. Finally , Section 5 concludes the paper . For clarity of presentation, the main proofs are provided in Appendix A and Appendix B. Notation: I and R denote the sets of integers and real numbers, respectiv ely , where superscripts or subscripts may be added to specify particular ranges (e.g., I ≥ 0 ). The matrices 0 n × m ∈ R n × m and I n ∈ R n × n denote the zero matrix and the identity matrix, respectively . For an y sequence u = { u 0 , u 1 , . . . , u N } , we denote by u a : b := { u a , . . . , u b } the subsequence from index a to b , where a and b are integers satisfying a ≤ b and a, b ∈ I 0: N . For any vector x ∈ R n , | x | denotes the Euclidean norm, and | x | 2 P is deﬁned as | x | 2 P := x ⊤ P x , where P ∈ R n × n is a symmetric matrix. A C-set is deﬁned as a conv ex, compact set containing the origin. Giv en a set A ⊂ R n and a matrix K ∈ R m × n , we deﬁne K A := { K a | a ∈ A } . A function belongs to class K if it is continuous, zero at zero, and strictly increasing; a function belongs to class K ∞ if it is in class K and unbounded. 2 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees T ime Input AAAB63icdVDLSsNAFJ34rPVVdelmsAh1E5Ja03ZXcOOygn1AG8pkOmmHzkzCzEQoob/gxoUibv0hd/6NkzaCih64cDjnXu69J4gZVdpxPqy19Y3Nre3CTnF3b//gsHR03FVRIjHp4IhFsh8gRRgVpKOpZqQfS4J4wEgvmF1nfu+eSEUjcafnMfE5mggaUox0JiWV2cWoVHZsZwno2Jde06tVDam79UbzCrq5VQY52qPS+3Ac4YQToTFDSg1cJ9Z+iqSmmJFFcZgoEiM8QxMyMFQgTpSfLm9dwHOjjGEYSVNCw6X6fSJFXKk5D0wnR3qqfnuZ+Jc3SHTY8FMq4kQTgVeLwoRBHcHscTimkmDN5oYgLKm5FeIpkghrE0/RhPD1KfyfdKu269neba3cauZxFMApOAMV4II6aIEb0AYdgMEUPIAn8Gxx69F6sV5XrWtWPnMCfsB6+wTQDo4Y u ( k ) AAAB/HicbVDLSsNAFJ3UV62vaJduBotQNyFRW9NdwY3LCvYBbQiT6aQdOnkwMxFLiL/ixoUibv0Qd/6NkzYLrR4YOJxzL/fM8WJGhTTNL620tr6xuVXeruzs7u0f6IdHPRElHJMujljEBx4ShNGQdCWVjAxiTlDgMdL3Zte5378nXNAovJPzmDgBmoTUpxhJJbl6NXHTUYDklAcpeZBZVp+duXrNNGy7cdEwoWlYttlqWoqYC0CrIDVQoOPqn6NxhJOAhBIzJMTQMmPppIhLihnJKqNEkBjhGZqQoaIhCohw0kX4DJ4qZQz9iKsXSrhQf26kKBBiHnhqMs8pVr1c/M8bJtK3nZSGcSJJiJeH/IRBGcG8CTimnGDJ5oogzKnKCvEUcYSl6quiSrBWv/yX9M4Nq2k0by9r7VZRRxkcgxNQBxa4Am1wAzqgCzCYgyfwAl61R+1Ze9Pel6Mlrdipgl/QPr4BiySVVw== u ext ( k ) Safety — Stability Set AAAB+XicdVDLSgMxFL1TX7W+Rl26CRah3Qwztda6K7hxWdE+oB1KJk3b0MyDJFMsQ//EjQtF3Pon7vwbM20FnwcCh3Pu5Z4cL+JMKtt+NzIrq2vrG9nN3Nb2zu6euX/QlGEsCG2QkIei7WFJOQtoQzHFaTsSFPsepy1vfJn6rQkVkoXBrZpG1PXxMGADRrDSUs80uz5WI89LbmaFu8K4WOyZeds6c06rFyX0mziWPUcelqj3zLduPySxTwNFOJay49iRchMsFCOcznLdWNIIkzEe0o6mAfapdJN58hk60UofDUKhX6DQXP26kWBfyqnv6ck0p/zppeJfXidWg6qbsCCKFQ3I4tAg5kiFKK0B9ZmgRPGpJpgIprMiMsICE6XLyukSPn+K/ifNkuVUrMp1OV8rL+vIwhEcQwEcOIcaXEEdGkBgAvfwCE9GYjwYz8bLYjRjLHcO4RuM1w/34JM2 S ( x ( k )) AAACN3icbVDLSgMxFM34tr6qLt0Ei1BBy4yIj4UguHElilaFTi2Z9LaGZjJDckes4/yVG3/DnW5cKOLWPzBTu/B14MLh3HNI7gliKQy67qMzMDg0PDI6Nl6YmJyaninOzp2aKNEcqjySkT4PmAEpFFRRoITzWAMLAwlnQWcv359dgTYiUifYjaEesrYSLcEZWqlRPEjKnWVKd6jPdNsPhWqk/g0go75Q1A8ZXgZBepyVr61tOaP+Cr1NrAXhGlM7WZbHV2kvc3ux1iiW3IrbA/1LvD4pkT4OG8UHvxnxJASFXDJjap4bYz1lGgWXkBX8xEDMeIe1oWapYiGYetq7O6NLVmnSVqTtKKQ99XsiZaEx3TCwzvwS83uXi//tagm2tuqpUHGCoPjXQ61EUoxoXiJtCg0cZdcSxrWwf6X8kmnG0VZdsCV4v0/+S07XKt5GZeNovbS73a9jjCyQRVImHtkku2SfHJIq4eSOPJEX8urcO8/Om/P+ZR1w+pl58gPOxycoRqrw u ( k ) = arg min ⇣ 2 S ( x ( k )) | u ext ( k )  ⇣ | 2 Figure 1: Conceptual illustration of the proposed PS 2 F framew ork. The external controller generates a goal-oriented command u ext , which may prioritise efﬁciency or high-lev el task performance but can potentially violate constraints or compromise stability . The PS 2 F framework acts as a predictive ﬁlter that projects u ext onto the safety–stability set S ( x ) (deﬁned in (16) and implicitly constructed via the two OCPs), ensuring that the applied control input remains both safe and stable. 2 Preliminaries 2.1 System Description Consider a non-afﬁne, nonlinear , time-in v ariant, discrete-time system given by x + = f ( x, u ) , (1) where x ∈ R n denotes the system state, u ∈ R m the control input, and x + the successor state. The system is subject to state and input constraints x ∈ X , u ∈ U , (2) where X ⊂ R n and U ⊂ R m are known C-sets, i.e., con v ex, compact sets containing the origin. Assume that at each time step k , a goal-oriented command u ext ( k ) ∈ R m is av ailable, which may be designed using either learning-based or data-dri ven methods b ut does not inherently guarantee safety or stability . The objecti ve of this work is to construct a safety–stability (S 2 )-set S ( x ) ⊆ U such that the ﬁltered control input u = arg min ζ ∈ S ( x ) | u ext − ζ | 2 (3) guarantees both constraint satisfaction and asymptotic stability of the resulting closed-loop system, for all admissible initial states within the feasible operating region. The S 2 -set S ( x ) is constructed implicitly through two cascaded OCPs, which are introduced sequentially in the following subsections. T o distinguish between them, we denote the control sequence by v and the predicted state by z ( i ; x ) := ϕ ( i ; x, v ) in the ﬁrst OCP (the nominal MPC), and by u and x ( i ; x ) := ϕ ( i ; x, u ) in the second OCP (the ﬁlter). Here, ϕ ( i ; x, · ) denotes the i -step state transition from the initial state x under the corresponding control sequence. By deﬁnition, x (0; x ) = z (0; x ) = x . 2.2 Nominal MPC as a Copilot The ﬁrst OCP is a nominal MPC problem, denoted by P N ( x ) , with prediction horizon N P N ( x ) : V ∗ N ( x ) := min v  V N ( x, v )   v ∈ V N ( x )  , (4) where the cost function V N ( x, v ) and the corresponding constraint set are deﬁned as V N ( x, v ) := N − 1 X i =0 ℓ ( z ( i ; x ) , v ( i ; x )) + V f ( z ( N ; x )) , V N ( x ) := n v | z ( i ; x ) ∈ X , v ( i ; x ) ∈ U , ∀ i ∈ I 0: N − 1 , z ( N ; x ) ∈ X f o . (5) 3 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees Here, X f denotes the terminal constraint set, while ℓ ( · , · ) and V f ( · ) represent the stage and terminal cost functions, respectiv ely . Let X N denote the set of initial states in X for which P N ( x ) is feasible, i.e., X N :=  x ∈ X   V N ( x )  = ∅  . (6) For an y x ∈ X N , the optimiser of P N ( x ) is giv en by v ∗ ( x ) = arg min v  V N ( x, v )   v ∈ V N ( x )  = { v ∗ (0; x ) , v ∗ (1; x ) , . . . , v ∗ ( N − 1; x ) } , (7) and the corresponding optimal state sequence is z ∗ ( x ) = { z ∗ (0; x ) , z ∗ (1; x ) , . . . , z ∗ ( N ; x ) } . (8) T o ensure recursi ve feasibility and asymptotic stability of the closed-loop system under the nominal MPC, the follo w- ing standard assumptions are imposed. For further details, the reader is referred to [21, Chap. 2.4]. Assumption 1 The functions f ( · , · ) : X × U → X , ℓ ( · , · ) : X × U → R ≥ 0 and V f ( · ) : X f → R ≥ 0 ar e continuous, with f (0 n × 1 , 0 m × 1 ) = 0 , ℓ (0 n × 1 , 0 m × 1 ) = 0 and V f (0 n × 1 ) = 0 . Assumption 2 The sets X , U , and X f ⊆ X ar e C-sets. Assumption 3 The stage cost ℓ ( · , · ) , the terminal cost V f ( · ) , and the terminal set X f satisfy the following pr operties: (a) F or all x ∈ X f , ther e exists a u ∈ U satisfying f ( x, u ) ∈ X f V f ( f ( x, u )) − V f ( x ) ≤ − ℓ ( x, u ) . (b) There e xists K ∞ functions α 1 ( · ) and α 2 ( · ) satisfying ℓ ( x, u ) ≥ α 1 ( | x | ) ∀ x ∈ X N , u ∈ U V f ( x ) ≤ α 2 ( | x | ) ∀ x ∈ X f . Proposition 1 ( [21, Pr oposition 2.16]) Suppose that Assumptions 1, 2, and 3 hold, and that the terminal set X f contains the origin in its interior . Then, there e xists a K ∞ function β such that V ∗ N ( x ) ≤ β ( | x | ) , ∀ x ∈ X N . Remark 1 T o guarantee safety and stability , the stage cost function in MPC is typically r equired to be non-negative and continuous (see Assumption 1). While these pr operties enable rigor ous analysis (such as r ecursive feasibility and asymptotic stability), they also imply that the nominal MPC cost may not directly encode the practical task that the system is meant to accomplish. In many applications, task-oriented objectives ar e inher ently discontinuous or non- smooth. F or instance, r einforcement-learning contr ollers commonly employ sparse or event-trigg ered r ewar d struc- tur es, whic h intr oduce discontinuities; logic-based or mode-switc hing tasks lead to piecewise or hybrid cost functions; and human-generated teleoperation inputs may not correspond to any continuous cost at all. These char acteristics make suc h task-driven objectives fundamentally incompatible with standar d MPC cost r equir ements. This is pr ecisely why the external command u ext ( k ) plays a crucial r ole in the proposed fr amework: it can originate fr om any source , e.g ., human intent, learning-based policies, heuristic planners, or discontinuous signals, and thus mor e faithfully cap- tur es the true task objective. The nominal MPC ther efore acts mer ely as a safety-stability copilot, ensuring constraint satisfaction and stability while allowing u ext ( k ) t o dictate the high-level objective . 3 Main Results 3.1 Predicti ve Safety–Stability Filter W ith the optimal sequences v ∗ ( x ) and z ∗ ( x ) obtained from (7) and (8), we can now introduce the proposed framew ork, the Pr edictive Safety–Stability F ilter (PS 2 F). Giv en any external command u ext , the second OCP is formulated as P f ,M ( x ) : V ∗ f ,M ( x ) := min u  V f ,M ( x, u )   u ∈ U a M ( x )  , (9) 4 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees External Controller Predictive Safety—Stability Filter AAACNHicbZBLSwMxFIUz9dFaX63iyk2wChVKmdHaqTvBjRuhgrVCW0qSybTBzIMkI5YwP8at7v0vgjtx628wU7vQ6oHA4dx7yeHDMWdS2farlVtYXFrOF1aKq2vrG5ul8taNjBJBaIdEPBK3GEnKWUg7iilOb2NBUYA57eK782zevadCsii8VpOYDgI0CpnPCFImGpZ29vsBUmOMdTsdar92mVYfDveHpYpdt6eCf40zMxUwU3tYtvJ9LyJJQENFOJKy59ixGmgkFCOcpsV+ImmMyB0a0Z6xIQqoHOhp/xQemMSDfiTMCxWcpj8vNAqknAS4hgOznPWV8+Ms/G/WS5TfGmgWxomiIfn+y084VBHMeECPCUoUnxiDiGCmLiRjJBBRhloR9j3qG7TTStobpVqMcKozFK7dbNTsunPSsFuuMcct12m6qUHnzIP6a26O6k6z3rw6qpydziAWwC7YA1XgABecgQvQBh1AgAaP4Ak8Wy/Wm/VufXyv5qzZzTb4JevzC/2SpSY= P f, M ( x ) Plant AAAB+XicbVDLSgNBEOyNrxhfqx69DAYhooRdkagHIeDFYwTzgGQNs5PZZMjs7DIzGxKW/IkXD4p49U+8+TdOHgdNLGgoqrrp7vJjzpR2nG8rs7K6tr6R3cxtbe/s7tn7BzUVJZLQKol4JBs+VpQzQauaaU4bsaQ49Dmt+/27iV8fUKlYJB71KKZeiLuCBYxgbaS2baPhU3o2RrcoKAzPUXLatvNO0ZkCLRN3TvIwR6Vtf7U6EUlCKjThWKmm68TaS7HUjHA6zrUSRWNM+rhLm4YKHFLlpdPLx+jEKB0URNKU0Giq/p5IcajUKPRNZ4h1Ty16E/E/r5no4NpLmYgTTQWZLQoSjnSEJjGgDpOUaD4yBBPJzK2I9LDERJuwciYEd/HlZVK7KLqlYunhMl++mceRhSM4hgK4cAVluIcKVIHAAJ7hFd6s1Hqx3q2PWWvGms8cwh9Ynz/SuZHT x + = f ( x, u ) AAACBnicbVDLSgNBEJyNrxhfqx5FGAxCBAm7IlEPQsCLxwjmAdkQZiezyZDZBzO9krDsyYu/4sWDIl79Bm/+jZNkD5pY0FBUddPd5UaCK7CsbyO3tLyyupZfL2xsbm3vmLt7DRXGkrI6DUUoWy5RTPCA1YGDYK1IMuK7gjXd4c3Ebz4wqXgY3MM4Yh2f9APucUpAS13zMMbX2BmSKCKl0SmOu4kDbASJrjQ96ZpFq2xNgReJnZEiylDrml9OL6SxzwKggijVtq0IOgmRwKlgacGJFYsIHZI+a2saEJ+pTjJ9I8XHWulhL5S6AsBT9fdEQnylxr6rO30CAzXvTcT/vHYM3mUn4UEUAwvobJEXCwwhnmSCe1wyCmKsCaGS61sxHRBJKOjkCjoEe/7lRdI4K9uVcuXuvFi9yuLIowN0hErIRheoim5RDdURRY/oGb2iN+PJeDHejY9Za87IZvbRHxifP2VwmHU= u =  ( x, u ext ) AAAB+HicbVDLSgNBEJyNrxgfWfXoZTAInsKuSNRbwIvHCOYBybLMTmaTIbMPZnrEuOyXePGgiFc/xZt/4yTZgyYWNBRV3XR3BangChzn2yqtrW9sbpW3Kzu7e/tV++CwoxItKWvTRCSyFxDFBI9ZGzgI1kslI1EgWDeY3Mz87gOTiifxPUxT5kVkFPOQUwJG8u0q1n42APYImak89+2aU3fmwKvELUgNFWj59tdgmFAdsRioIEr1XScFLyMSOBUsrwy0YimhEzJifUNjEjHlZfPDc3xqlCEOE2kqBjxXf09kJFJqGgWmMyIwVsveTPzP62sIr7yMx6kGFtPFolALDAmepYCHXDIKYmoIoZKbWzEdE0komKwqJgR3+eVV0jmvu4164+6i1rwu4iijY3SCzpCLLlET3aIWaiOKNHpGr+jNerJerHfrY9FasoqZI/QH1ucPci+TmQ== u ext AAAB+XicbVDLSsNAFL3xWesr6tLNYBGqi5KIVN0V3LisYB/QxjKZTtqhk0mYmRRL6J+4caGIW//EnX/jpM1CWw8MHM65l3vm+DFnSjvOt7Wyura+sVnYKm7v7O7t2weHTRUlktAGiXgk2z5WlDNBG5ppTtuxpDj0OW35o9vMb42pVCwSD3oSUy/EA8ECRrA2Us+2uyHWQz9Ix9PH8/LTGerZJafizICWiZuTEuSo9+yvbj8iSUiFJhwr1XGdWHsplpoRTqfFbqJojMkID2jHUIFDqrx0lnyKTo3SR0EkzRMazdTfGykOlZqEvpnMcqpFLxP/8zqJDq69lIk40VSQ+aEg4UhHKKsB9ZmkRPOJIZhIZrIiMsQSE23KKpoS3MUvL5PmRcWtVqr3l6XaTV5HAY7hBMrgwhXU4A7q0AACY3iGV3izUuvFerc+5qMrVr5zBH9gff4AnbmS+g== v ⇤ ( x ) AAAB+XicbVDLSgMxFL1TX7W+Rl26CRahuigzIlV3BTcuK9gHtLVk0kwbmskMSaZYh/6JGxeKuPVP3Pk3ZtpZaOuBwOGce7knx4s4U9pxvq3cyura+kZ+s7C1vbO7Z+8fNFQYS0LrJOShbHlYUc4ErWumOW1FkuLA47TpjW5SvzmmUrFQ3OtJRLsBHgjmM4K1kXq23QmwHnp+8jR9OCs9nqKeXXTKzgxombgZKUKGWs/+6vRDEgdUaMKxUm3XiXQ3wVIzwum00IkVjTAZ4QFtGypwQFU3mSWfohOj9JEfSvOERjP190aCA6UmgWcm05xq0UvF/7x2rP2rbsJEFGsqyPyQH3OkQ5TWgPpMUqL5xBBMJDNZERliiYk2ZRVMCe7il5dJ47zsVsqVu4ti9TqrIw9HcAwlcOESqnALNagDgTE8wyu8WYn1Yr1bH/PRnJXtHMIfWJ8/o+WS/g== z ⇤ ( x ) AAAB6HicbVDLSgNBEOyNrxhfUY9eBoPgKeyKRL0FvHhMwDwgWcLspDcZMzu7zMyKIeQLvHhQxKuf5M2/cZLsQRMLGoqqbrq7gkRwbVz328mtrW9sbuW3Czu7e/sHxcOjpo5TxbDBYhGrdkA1Ci6xYbgR2E4U0igQ2ApGtzO/9YhK81jem3GCfkQHkoecUWOl+lOvWHLL7hxklXgZKUGGWq/41e3HLI1QGiao1h3PTYw/ocpwJnBa6KYaE8pGdIAdSyWNUPuT+aFTcmaVPgljZUsaMld/T0xopPU4CmxnRM1QL3sz8T+vk5rw2p9wmaQGJVssClNBTExmX5M+V8iMGFtCmeL2VsKGVFFmbDYFG4K3/PIqaV6UvUq5Ur8sVW+yOPJwAqdwDh5cQRXuoAYNYIDwDK/w5jw4L86787FozTnZzDH8gfP5A+cWjP4= x MPC as a Copilot AAACMnicbZBLSwMxFIUz9VXrq9qlm2ArVChlxken7gQ3rqSCtYW2lCSTaUMzD5KMWML8Fre698/oTtz6I8zULtR6IHA4915y+HDMmVS2/WrllpZXVtfy64WNza3tneLu3p2MEkFom0Q8El2MJOUspG3FFKfdWFAUYE47eHKZzTv3VEgWhbdqGtNBgEYh8xlBykTDYqnSD5AaY6xb6VBfp9WHo8qwWLbr9kxw0ThzUwZztYa71lrfi0gS0FARjqTsOXasBhoJxQinaaGfSBojMkEj2jM2RAGVAz1rn8JDk3jQj4R5oYKz9OeFRoGU0wDXcGCWs7by7zgL/5v1EuU3B5qFcaJoSL7/8hMOVQQzGtBjghLFp8YgIpipC8kYCUSUYVaAfY/6BuyskvZGqRYjnOoMhWs3Tmt23Tk7tZuuMSdN12m4qUHn/AW1aO6O606j3rg5Ll+czyHmwT44AFXgABdcgCvQAm1AwBQ8gifwbL1Yb9a79fG9mrPmNyXwS9bnF6qApIE= P N ( x ) Figure 2: Schematic of the proposed PS 2 F framew ork. The nominal MPC serves as a copilot , generating safety- and stability-certiﬁed trajectories. The ﬁltering OCP P f ,M ( · ) then reﬁnes the external command within a safe and stable region before applying the ﬁnal control action to the system plant. The dashed arrow above the external controller indicates that the external command may be independent of the system state, i.e., operating in open-loop mode. where the cost function V f ,M ( x, u ) and the corresponding constraint set U a M ( x ) are deﬁned as V f ,M ( x, u ) := | u ext − u (0; x ) | 2 U a M ( x ) := n u    x ( i ; x ) ∈ X , u ( i ; x ) ∈ U , ∀ i ∈ I 0: M − 1 , (11a) and (11b) o . (10) Note that the external command u ext is embedded directly into the cost function of P f ,M ( x ) . The performance constraint and terminal constraint are giv en respectiv ely by L ( x 0: M − 1 ( x ) , u ( x )) − L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) − aℓ ( x (0; x ) , u (0; x )) ≤ 0 , (11a) x ( M ; x ) = z ∗ ( M ; x ) , (11b) where a ≥ 0 is a tunable parameter and M ∈ I 1: N is the prediction horizon of the second OCP . The function L ( · , · ) represents the cumulativ e stage cost over the ﬁrst M steps, deﬁned as L ( x 0: M − 1 ( x ) , u ( x )) := M − 1 X i =0 ℓ  x ( i ; x ) , u ( i ; x )  . (12) Similarly , we ha ve L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) = M − 1 X i =0 ℓ  z ∗ ( i ; x ) , v ∗ ( i ; x )  . The optimiser of P f ,M ( x ) is denoted by u ∗ ( x ) = arg min u  V f ,M ( x, u )   u ∈ U a M ( x )  = { u ∗ (0; x ) , u ∗ (1; x ) , . . . , u ∗ ( M − 1; x ) } , (13) and the corresponding optimal state sequence is x ∗ ( x ) = { x ∗ (0; x ) , x ∗ (1; x ) , . . . , x ∗ ( M ; x ) } . (14) Finally , the ﬁltered control la w is deﬁned as u = κ ( x, u ext ) := u ∗ (0; x ) . (15) Since only the ﬁrst control component inﬂuences the cost function, (15) is equiv alent to (3) through the deﬁnition of the S 2 -set S ( x ) , giv en by S ( x ) := { u (0; x ) | u ∈ U a M ( x ) } . (16) The complete control procedure is presented in Algorithm 1, while Fig. 2 illustrates the overall structure of the pro- posed PS 2 F framew ork. Before analysing the closed-loop behaviour , we ﬁrst establish several fundamental properties of the second OCP , P f ,M ( x ) , including its feasibility and the role of the tunable parameters a and M . Unless otherwise speciﬁed, As- sumptions 1, 2, and 3 are assumed to hold throughout this subsection. For readability , the proofs of the following results are provided in Appendix A. 5 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees Algorithm 1 PS 2 F 1: Ofﬂine: Specify prediction horizons N and M ≤ N ; cost functions ℓ ( · , · ) and V f ( · ) ; constraint sets X , U , X f ; and parameter a ∈ [0 , 1) . 2: Initialisation: Set time k ← 0 and measure x (0) . 3: Step One-Nominal MPC: Solve P N  x ( k )  to obtain v ∗ ( x ( k )) and z ∗ ( x ( k )) . 4: Step T wo-Receiv e external command: Obtain u ext ( k ) fr om any goal-oriented controller . 5: Step Three-PS 2 F: Solve P f ,M  x ( k )  to obtain u ∗ ( x ( k )) and x ∗ ( x ( k )) . Set the ﬁltered control input u ( k ) ← u ∗ (0; x ( k )) (cf. (15)). 6: Step Four -Apply and pr opagate: Apply u ( k ) to the system (1) to obtain x ( k +1) = f  x ( k ) , u ( k )  . 7: k ← k + 1 and go to Step One . Proposition 2 P f ,M ( x ) is feasible for all x ∈ X N ; that is, U a M ( x )  = ∅ for every x ∈ X N . Proposition 3 Suppose that x ∈ X N and a = 0 . (a) The below equality holds: L ( x ∗ 0: M − 1 ( x ) , u ∗ ( x )) = L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) . (b) Moreover , if the OCP P N ( x ) is strictly con vex, then the ﬁltered contr ol law r educes to the nominal MPC action. In particular , u = u ∗ (0; x ) = v ∗ (0; x ) , and the S 2 -set de generates to a singleton: S ( x ) = { v ∗ (0; x ) } . Proposition 4 Suppose that x ∈ X N and a 1 ≥ a 2 ≥ 0 . Then, the corr esponding S 2 -sets satisfy the nested inclusion S a 2 ( x ) ⊆ S a 1 ( x ) , wher e S a i ( x ) denotes the S 2 -set associated with the parameter a = a i . Proposition 5 Suppose that x ∈ X N and M = 1 . Assume that the mapping u 7→ f ( x, u ) is injective on U . Then, the ﬁlter ed contr ol law reduces to the nominal MPC action. In particular , u (0; x ) = u ∗ (0; x ) = v ∗ (0; x ) , and the S 2 -set de generates to a singleton: S ( x ) = { v ∗ (0; x ) } . Proposition 6 Suppose that x ∈ X N and i ≥ j , where i, j ∈ I 1: N . Then, the corresponding S 2 -sets satisfy the nested inclusion S j ( x ) ⊆ S i ( x ) , wher e S i ( x ) denotes the S 2 -set associated with pr ediction horizon M = i . Remark 2 Proposition 2 shows that the ﬁlter pr oblem P f ,M ( x ) does not r equire any additional assumptions beyond those of the nominal MPC pr oblem P N ( x ) . As long as x ∈ X N , the ﬁlter r emains feasible. Ther efor e, classical techniques for enlar ging the r egion of attraction X N (e.g ., [22]) can be dir ectly employed to e xtend the safe and stable operating r e gion. Remark 3 Propositions 3, 4, 5, and 6 jointly char acterise how the design parameter s a and M determine the geometry of the S 2 -set. Pr oposition 3 shows that when a = 0 , the S 2 -set collapses to the nominal MPC action. Pr oposition 4 establishes that the S 2 -set is monotone in a , with larg er values producing lar ger sets. Similarly , Pr opositions 5 and 6 show that the pr ediction horizon M also induces a monotone expansion of the S 2 -set, and that for M = 1 the ﬁltered input coincides with the nominal MPC contr ol. T aken together , these r esults demonstrate that incr easing either a or M reduces conservatism by enlar ging the S 2 -set, wher eas smaller values mak e the ﬁlter behave mor e lik e the nominal MPC law . This tr end is further illustrated in the numerical examples pr esented later . 6 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees T o more clearly illustrate the geometry of the S 2 -set S ( x ) , we consider a representative example based on a general linear system. Although it was previously deﬁned only implicitly through the two OCPs, in this example, we explicitly deriv e the analytical description of S ( x ) , making its structure transparent. Proposition 7 Suppose that f ( x, u ) = Ax + B u , where A ∈ R n × n and B ∈ R n × m ar e system matrices, and ( A, B ) is contr ollable. The nominal MPC is designed with the quadratic stage and terminal costs, given by ℓ ( x, u ) = x ⊤ Qx + u ⊤ Ru, V f ( x ) = x ⊤ P x, wher e Q > 0 , R > 0 , and P > 0 is the positive deﬁnite solution of the discr ete-time algebraic Riccati equation associated with ( A, B , Q, R ) . W e further assume that the safety constraints in both OCPs, as well as the terminal constraint of the ﬁrst OCP , ar e inactive. Then, the admissible input set U a M ( x ) is the inter section of an af ﬁne subspace and a quadratic set, i.e ., an ellipsoid sliced by a linear constraint: U a M ( x ) = n u ∈ R M m    A eq u = b eq x, u ⊤ H u + 2 x ⊤ F u + x ⊤ Gx ≤ 0 o , and hence, S ( x ) = e 1 U a M ( x ) , wher e A eq , b eq , H , F , G , and e 1 ar e constant matrices whose explicit e xpressions ar e pr ovided in Appendix A. 3.2 Theoretical Analysis In this subsection, we analyse the closed-loop properties of the proposed PS 2 F frame work. W e begin with the nominal case, where all parameters are held constant, and then extend the analysis to the mode-scheduling case, in which the parameters vary over time. For completeness, the proofs of all theoretical results presented in this subsection are provided in Appendix B. Theorem 1 Suppose that Assumptions 1, 2, and 3 hold, and that the terminal set X f contains the origin in its interior . Let the initial state satisfy x (0) ∈ X N . Then, for any external command u ext ( k ) , if a ∈ [0 , 1) , the system (1) under the contr ol law (15) satisﬁes the following pr operties: (a) The safety constraints ar e satisﬁed for all k ∈ I ≥ 0 . (b) The closed-loop system is asymptotically stable. Based on Theorem 1, we no w extend the proposed frame work to the time-varying case, in which the design parameters a ( k ) and M ( k ) e volve ov er time (see Algorithm 2). This extension enables the controller to adapt its behaviour dynamically , i.e., prioritising safety during exploration and progressi vely enforcing stability during exploitation. The following theorem formalises these properties. Theorem 2 Suppose that Assumptions 1, 2, and 3 hold, and that the terminal set X f contains the origin in its interior . Let the initial state satisfy x (0) ∈ X N . Then, for any given external command u ext ( k ) , if M ( k ) ∈ I 1: N and a ( k ) ∈ [0 , ∞ ) , the system (1) under the contr ol law (15) satisﬁes the following pr operties: (a) The safety constraints ar e satisﬁed for all k ∈ I ≥ 0 . (b) Furthermore, if there exists a time instant K s ∈ I ≥ 0 such that sup k ≥ K s a ( k ) < 1 , then the closed-loop system is asymptotically stable for all k ∈ I ≥ K s . Remark 4 Theorem 2 pr ovides the theoretical foundation for r esolving the classical exploration–e xploitation dilemma within a uniﬁed predictive contr ol framework. Property (a) guar antees that the system remains safe even when stability is intentionally r elaxed (e.g., by choosing a ( k ) ≥ 1 ), ther eby enabling the contr oller to explor e a broader range of contr ol actions or state trajectories without violating safety constraints—this corr esponds to the safe exploration mode. In contrast, Pr operty (b) ensures that, after any ﬁnite exploration phase , the system trajectories asymptotically con ver ge once the contr oller switches to the exploitation mode (by per sistently enforcing a ( k ) ≤ 1 − ¯ a , with ¯ a ∈ (0 , 1] ), wher e stability dominates. Differ ent fr om the parameter a ( k ) , which directly determines the stability behaviour of the closed-loop system, the horizon parameter M ( k ) does not explicitly inﬂuence stability . Nevertheless, M ( k ) plays a complementary and practically important r ole: increasing M ( k ) enlar ges the S 2 -set, ther eby r educing conservatism and allowing the contr oller to pr eserve mor e of the external command. This, however , comes at the price of higher computational com- plexity . Con versely , smaller values of M ( k ) shrink the S 2 -set, making the ﬁlter mor e restrictive but computationally 7 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees Algorithm 2 PS 2 F with Mode Scheduling 1: Ofﬂine: Specify prediction horizons N and M ( k ) ∈ I 1: N ; cost functions ℓ ( · , · ) and V f ( · ) ; constraint sets X , U , X f ; and mode-scheduling parameter a ( k ) ≥ 0 . 2: Initialisation: Set time k ← 0 and measure x (0) . 3: Step One-Nominal MPC: Solve P N  x ( k )  to obtain v ∗ ( x ( k )) and z ∗ ( x ( k )) . 4: Step T wo-Receiv e external command: Obtain u ext ( k ) from an y goal-oriented controller . 5: Step Three-Mode scheduling: Adjust a ( k ) according to the desired operation mode: 6: if safe exploration phase then 7: Choose a large a ( k ) ≥ 1 to prioritise constraint satisfaction (safety-ﬁrst mode). 8: else if stable exploitation phase then 9: Choose a small a ( k ) ≤ 1 − ¯ a (with ¯ a ∈ (0 , 1] ) to enhance asymptotic stability . 10: end if 11: Step Four -PS 2 F: Receiv e the scheduled parameters a ( k ) and M ( k ) . Solve P f ,M  x ( k )  to obtain u ∗ ( x ( k )) and x ∗ ( x ( k )) . Set the ﬁltered control input u ( k ) ← u ∗ (0; x ( k )) (cf. (15)). 12: Step Five-Apply and pr opagate: Apply u ( k ) to the system (1) to obtain x ( k +1) = f  x ( k ) , u ( k )  . 13: k ← k + 1 and go to Step One . cheaper . Adjusting both a ( k ) and M ( k ) thus pro vides a principled mechanism for shaping how the system transi- tions fr om exploration to e xploitation. This capability to safely transition between e xploration and exploitation phases within a single, theoretically grounded framework r epresents a central contribution of the pr oposed PS 2 F appr oach. This featur e will be further illustrated in the r obotic navigation task in the following section. Remark 5 In situations where the terminal set X f does not have an interior , for example when X f = { 0 n × 1 } , the upper bound in Pr oposition 1 cannot be established, and thus the asymptotic stability in the Lyapunov sense cannot be concluded dir ectly [21, Chap. 2.4]. Nevertheless, con ver gence of the state to the origin can still be guaranteed. Since V ∗ N ( x ( k )) is nonincr easing along closed-loop trajectories (see (33) or (35) ) and satisﬁes V ∗ N ( x ( k )) ≥ 0 for all k , the Monotone Con ver gence Theor em implies that V ∗ N ( x ( k )) conver ges to a ﬁnite limit as k → ∞ . Mor eover , the decrease condition for ces this limit to be zer o, which in turn implies that x ( k ) con ver ges to the origin. 4 Simulation In this section, we validate the proposed PS 2 F framework through three simulation studies. Each case study high- lights a different aspect of the method: the evolution of the S 2 -set over time, the inﬂuence of design parameters on conservatism, and the integration of PS 2 F with a goal-oriented external controller in robotic navigation. These simula- tions demonstrate how the proposed ﬁlter consistently enforces safety and stability while allo wing ﬂexible, high-le vel control behaviours. 4.1 Case Study 1: Evolution Over T ime Consider the linear system x + = Ax + B u , with A =  1 1 0 1  , B =  1 0 0 1  , subject to the state and input constraints X = [ − 2 , 2] 2 , U = [ − 1 , 1] 2 . The initial state is chosen as x (0) = [2 , − 2] ⊤ . The MPC stage cost is deﬁned as ℓ ( x, u ) = x ⊤ Qx + u ⊤ Ru with Q = 10 I 2 , R = I 2 , and prediction horizon N = 5 . The terminal cost and terminal set are chosen as V f ( x ) = x ⊤ P x, X f = { x ∈ R 2 | x ⊤ P x ≤ γ } , where P =  10 . 92 0 . 92 0 . 92 11 . 85  8 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees 0.5 5 10 15 -1 20 25 30 -0.5 0 0 -0.5 0.5 1 AAAB+XicbVBNT8JAFHzFL8SvqkcvG4kJXEhrDOqNxItHjIIk0JDtsoUN222zuyWShn/ixYPGePWfePPfuIUeFJxkk8nMe3mz48ecKe0431ZhbX1jc6u4XdrZ3ds/sA+P2ipKJKEtEvFIdnysKGeCtjTTnHZiSXHoc/roj28y/3FCpWKReNDTmHohHgoWMIK1kfq23QuxHvl+ej+rPFXG1WrfLjs1Zw60StyclCFHs29/9QYRSUIqNOFYqa7rxNpLsdSMcDor9RJFY0zGeEi7hgocUuWl8+QzdGaUAQoiaZ7QaK7+3khxqNQ09M1kllMte5n4n9dNdHDlpUzEiaaCLA4FCUc6QlkNaMAkJZpPDcFEMpMVkRGWmGhTVsmU4C5/eZW0z2tuvVa/uyg3rvM6inACp1ABFy6hAbfQhBYQmMAzvMKblVov1rv1sRgtWPnOMfyB9fkDf+SS5w== S ( x ( k )) AAAB63icbVBNSwMxEJ2tX7V+VT16CRahXsqulKq3ghePFewHtEvJptk2NMkuSVYoS/+CFw+KePUPefPfmG33oK0PBh7vzTAzL4g508Z1v53CxubW9k5xt7S3f3B4VD4+6egoUYS2ScQj1QuwppxJ2jbMcNqLFcUi4LQbTO8yv/tElWaRfDSzmPoCjyULGcEmk5Lq9HJYrrg1dwG0TrycVCBHa1j+GowikggqDeFY677nxsZPsTKMcDovDRJNY0ymeEz7lkosqPbTxa1zdGGVEQojZUsatFB/T6RYaD0Tge0U2Ez0qpeJ/3n9xIQ3fspknBgqyXJRmHBkIpQ9jkZMUWL4zBJMFLO3IjLBChNj4ynZELzVl9dJ56rmNWqNh3qleZvHUYQzOIcqeHANTbiHFrSBwASe4RXeHOG8OO/Ox7K14OQzp/AHzucPcGSN1Q== u ( k ) AAAB/HicbVDLSgMxFM3UV62v0S7dBItQN2VGpOqu4MZlBfuAdhgyaaYNTTJDkhGHof6KGxeKuPVD3Pk3ZtpZaOuBwOGce7knJ4gZVdpxvq3S2vrG5lZ5u7Kzu7d/YB8edVWUSEw6OGKR7AdIEUYF6WiqGenHkiAeMNILpje533sgUtFI3Os0Jh5HY0FDipE2km9XEz8bcqQnkmfkUc9m9emZb9echjMHXCVuQWqgQNu3v4ajCCecCI0ZUmrgOrH2MiQ1xYzMKsNEkRjhKRqTgaECcaK8bB5+Bk+NMoJhJM0TGs7V3xsZ4kqlPDCTeU617OXif94g0eGVl1ERJ5oIvDgUJgzqCOZNwBGVBGuWGoKwpCYrxBMkEdamr4opwV3+8irpnjfcZqN5d1FrXRd1lMExOAF14IJL0AK3oA06AIMUPINX8GY9WS/Wu/WxGC1ZxU4V/IH1+QM74JUh u ext ( k ) Figure 3: Three-dimensional illustration of the PS 2 F operation over time. The S 2 -set S ( x ( k )) (grey surface) evolv es with the system state, while the external command u ext ( k ) (red circle) and the ﬁltered control u ( k ) (blue square) are visualised along the time axis. is the positiv e deﬁnite solution of the discrete-time algebraic Riccati equation associated with ( A, B , Q, R ) , and γ = 7 . 28 is selected so that X f is the largest ellipsoid contained within the state and input constraints and in variant under the terminal controller u = − K LQR x . It is straightforward to verify that this MPC setup satisﬁes Assumptions 1, 2, and 3. The PS 2 F parameters are chosen as M = 2 and α = 0 . 95 . T o make the scenario more challenging, we choose the external command as u ext ( k ) =  u 1 , ext ( k ) u 2 , ext ( k )  =  − 1 . 2 cos(0 . 2 k + 0 . 2) 0 . 1 x 2 ( k )  . The ﬁrst component u 1 , ext ( k ) is an open-loop signal that may violate the input constraints, while the second com- ponent u 2 , ext ( k ) corresponds to an unstable feedback law . Indeed, applying u 2 , ext ( k ) directly to the system leads to x 2 ( k + 1) = 1 . 1 x 2 ( k ) , which div erges ov er time and is therefore intrinsically unstable. The results are shown in Figs. 3 and 4. Fig. 3 provides a macroscopic, time-ev olving visualisation of the PS 2 F mechanism. It illustrates how the S 2 -set S ( x ( k )) ev olves as the system state changes, forming a grey surf ace in the 3D space spanned by ( u 1 , u 2 , k ) . Superimposed on this surface are the trajectories of the external command u ext ( k ) and the ﬁltered control u ( k ) , which clearly sho w ho w unsafe or unstable external actions are projected onto the safe–stable region. Fig. 4 complements this with a microscopic, step-by-step perspecti ve. Each panel sho ws a 2D cross-section of the S 2 -set at a giv en time k , together with the external command, the ﬁltered control, and the direction of correction applied by the ﬁlter . These snapshots also align with Proposition 7: as x ( k ) approaches the origin, the corresponding S 2 -set S ( x ( k )) progressively shrinks, ev entually collapsing to the singleton { 0 m × 1 } . Overall, these visualisations demonstrate how PS 2 F systematically modiﬁes the external command at each time step and how the geometry of S ( x ( k )) gov erns the ﬁlter’ s corrective beha viour . 9 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 (a) k = 0 (b) k = 1 (c) k = 2 (d) k = 3 (e) k = 4 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 (f) k = 5 (g) k = 6 (h) k = 7 (i) k = 8 (j) k = 9 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 (k) k = 10 (l) k = 15 (m) k = 30 (n) k = 70 (o) k = 99 Figure 4: Evolution of the PS 2 F ﬁltering process along the u 1 – u 2 axes. Each panel shows the S 2 -set S ( x ( k )) (grey surface), the external command u ext ( k ) (red circle), and the ﬁltered control u ( k ) (blue square). The orange arrow depicts the correctiv e action generated by PS 2 F , which corresponds to the perpendicular projection of u ext ( k ) onto the admissible set. This projection direction is orthogonal to the local tangent of the boundary ∂ S ( x ( k )) (purple dash-dotted line), ensuring that the ﬁltered input remains within the safety–stability domain while staying as close as possible to the external command. 4.2 Case Study 2: Evolution Over Parameters T o in vestigate how the design parameters inﬂuence the geometry of the S 2 -set and thus the le vel of conservatism of the PS 2 F , we systematically v ary the key parameters and plot the resulting S 2 -sets at the same state. For consistency , this parametric study is conducted on the same linear system used in Case Study 1, and we only focus on the case where the state is in its initial condition x = x (0) = [2 , − 2] ⊤ . W e begin with the parameters of the PS 2 F . Fig. 5 illustrates the inﬂuence of the parameter a with M = 5 ﬁxed. In line with Propositions 3 and 4, increasing a yields a larger S 2 -set. Fig. 6 sho ws the effect of the prediction horizon M with a = 0 . 95 ﬁxed. As predicted by Propositions 5 and 6, the S 2 -set expands monotonically as M increases. A longer predicti ve horizon provides more de grees of freedom for constructing safe–stable trajectories, thereby reducing conservatism and enlarging the set of admissible ﬁltered inputs. A concise summary of these parameter effects is provided in T able 1. W e next inv estigate how the design parameters of the nominal MPC, which forms the foundation of PS 2 F , affect the geometry of the resulting S 2 -set. Figure 7 illustrates the inﬂuence of the MPC state-weighting matrix, where we vary Q = ρI 2 while ﬁxing R = I 2 and M = N = 5 . These results show ho w shaping the nominal cost landscape directly alters the boundary of the safe–stable domain. Figure 8 then examines the effect of the MPC prediction horizon N , with M = N ﬁxed. As N increases, the S 2 -set e xpands because the re gion of attraction of the nominal MPC becomes larger , enabling the ﬁlter to preserve a wider range of external commands while still guaranteeing safety and stability . 10 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees T able 1: Effect of increasing each PS 2 F design parameter . Param. Effect on S ( x ) Side Effect a Enlarges S ( x ) T ypically slower con vergence M Enlarges S ( x ) Higher computational cost -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 AAAB7HicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEqh6EghePFUxbaEPZbDft0s0m7E6EUvobvHhQxKs/yJv/xm2bg7Y+GHi8N8PMvDCVwqDrfjuFtfWNza3idmlnd2//oHx41DRJphn3WSIT3Q6p4VIo7qNAydup5jQOJW+Fo7uZ33ri2ohEPeI45UFMB0pEglG0kk9vPdftlStu1Z2DrBIvJxXI0eiVv7r9hGUxV8gkNabjuSkGE6pRMMmnpW5meErZiA54x1JFY26CyfzYKTmzSp9EibalkMzV3xMTGhszjkPbGVMcmmVvJv7ndTKMroOJUGmGXLHFoiiTBBMy+5z0heYM5dgSyrSwtxI2pJoytPmUbAje8surpHlR9WrV2sNlpX6Tx1GEEziFc/DgCupwDw3wgYGAZ3iFN0c5L86787FoLTj5zDH8gfP5A5L5jd0= a = 100 AAAB7HicbVBNS8NAEJ3Ur1q/qh69LBbBU0hEqh6EghePFUxbaEPZbDft0s1u2N0IJfQ3ePGgiFd/kDf/jds2B219MPB4b4aZeVHKmTae9+2U1tY3NrfK25Wd3b39g+rhUUvLTBEaEMml6kRYU84EDQwznHZSRXEScdqOxnczv/1ElWZSPJpJSsMEDwWLGcHGSgG+9dx6v1rzXG8OtEr8gtSgQLNf/eoNJMkSKgzhWOuu76UmzLEyjHA6rfQyTVNMxnhIu5YKnFAd5vNjp+jMKgMUS2VLGDRXf0/kONF6kkS2M8FmpJe9mfif181MfB3mTKSZoYIsFsUZR0ai2edowBQlhk8swUQxeysiI6wwMTafig3BX355lbQuXL/u1h8ua42bIo4ynMApnIMPV9CAe2hCAAQYPMMrvDnCeXHenY9Fa8kpZo7hD5zPH5eBjeA= a =0 . 6 AAAB7HicbVBNS8NAEJ3Ur1q/qh69LBbBU0hUqh6EghePFUxbaEPZbDft0t1N2N0IJfQ3ePGgiFd/kDf/jds2B60+GHi8N8PMvCjlTBvP+3JKK6tr6xvlzcrW9s7uXnX/oKWTTBEakIQnqhNhTTmTNDDMcNpJFcUi4rQdjW9nfvuRKs0S+WAmKQ0FHkoWM4KNlQJ847nn/WrNc7050F/iF6QGBZr96mdvkJBMUGkIx1p3fS81YY6VYYTTaaWXaZpiMsZD2rVUYkF1mM+PnaITqwxQnChb0qC5+nMix0LriYhsp8BmpJe9mfif181MfBXmTKaZoZIsFsUZRyZBs8/RgClKDJ9Ygoli9lZERlhhYmw+FRuCv/zyX9I6c/26W7+/qDWuizjKcATHcAo+XEID7qAJARBg8AQv8OpI59l5c94XrSWnmDmEX3A+vgGS9Y3d a =0 . 3 AAAB6nicbVBNS8NAEJ3Ur1q/qh69LBbBU0lEqh6EghePFe0HtKFMtpt26WYTdjdCCf0JXjwo4tVf5M1/47bNQasPBh7vzTAzL0gE18Z1v5zCyura+kZxs7S1vbO7V94/aOk4VZQ1aSxi1QlQM8ElaxpuBOskimEUCNYOxjczv/3IlOaxfDCThPkRDiUPOUVjpXu8dvvlilt15yB/iZeTCuRo9MufvUFM04hJQwVq3fXcxPgZKsOpYNNSL9UsQTrGIetaKjFi2s/mp07JiVUGJIyVLWnIXP05kWGk9SQKbGeEZqSXvZn4n9dNTXjpZ1wmqWGSLhaFqSAmJrO/yYArRo2YWIJUcXsroSNUSI1Np2RD8JZf/ktaZ1WvVq3dnVfqV3kcRTiCYzgFDy6gDrfQgCZQGMITvMCrI5xn5815X7QWnHzmEH7B+fgGtEaNaA== a =0 AAAB7XicbVBNSwMxEJ31s9avqkcvwSJ4WnZFqz0IBS8eK9gPaJeSTbNtbDZZkqxQlv4HLx4U8er/8ea/MW33oK0PBh7vzTAzL0w408bzvp2V1bX1jc3CVnF7Z3dvv3Rw2NQyVYQ2iORStUOsKWeCNgwznLYTRXEcctoKR7dTv/VElWZSPJhxQoMYDwSLGMHGSk1847nVy16p7LneDGiZ+DkpQ456r/TV7UuSxlQYwrHWHd9LTJBhZRjhdFLsppommIzwgHYsFTimOshm107QqVX6KJLKljBopv6eyHCs9TgObWeMzVAvelPxP6+Tmug6yJhIUkMFmS+KUo6MRNPXUZ8pSgwfW4KJYvZWRIZYYWJsQEUbgr/48jJpnrt+xa3cX5Rr1TyOAhzDCZyBD1dQgzuoQwMIPMIzvMKbI50X5935mLeuOPnMEfyB8/kDEpGOIg== a =0 . 95 Figure 5: Effect of parameter a on S ( x ) , with N = M = 5 , Q = 10 I 2 , and R = I 2 . Notably , when N = 1 , the nominal MPC problem becomes infeasible, highlighting the critical role of the prediction horizon in ensuring feasibility of the ov erall PS 2 F framew ork. 4.3 Case Study 3: Go–Stay–Return Na vigation In this ﬁnal case study , we demonstrate the applicability of the PS 2 F framework in a dynamic navigation task that requires both transient manoeuvring and temporary station-keeping. W e consider a nonholonomic mobile robot with unicycle-type kinematics [23, 24], whose dynamics are giv en by " p x ( k + 1) p y ( k + 1) θ ( k + 1) # = " p x ( k ) p y ( k ) θ ( k ) # + T s " cos( θ ( k )) 0 sin( θ ( k )) 0 0 1 #  v ( k ) ω ( k )  (17) where ( p x ( k ) , p y ( k )) denotes the robot’ s planar position, θ ( k ) its heading angle, v ( k ) the forward linear veloc- ity , ω ( k ) the angular velocity , and T s = 0 . 2 s is the sampling time. Let x ( k ) = [ p x ( k ) , p y ( k ) , θ ( k )] ⊤ and u ( k ) = [ v ( k ) , ω ( k )] ⊤ denote the state and input vectors, respectiv ely , with the initial condition x (0) = 0 3 × 1 . This model captures the fundamental nonholonomic constraint of wheeled robots: the robot moves only in the direction of its heading and must reorient via ω ( k ) to change direction. The unicycle model is widely used for ground robots, differential-dri ve v ehicles, and autonomous mobile platforms operating in planar environments. The task in this case study is to guide the mobile robot (17) from its initial position to a designated target region, remain within that re gion for a prescribed dwell time, and subsequently return safely to its original location. This three-phase “go–stay–return” behaviour is representative of many practical robotic missions, including inspection–pause–return tasks, waypoint surveillance, and object pickup followed by retreat [25]. Such tasks require not only accurate motion ex ecution during the outbound and return phases, b ut also the ability to maintain a stable stationary posture within the target region. This combination of manoeuvring, dwelling, and safe return makes the scenario particularly well-suited for ev aluating the mode scheduling capability of the PS 2 F framework. It is worth noting that this “go–stay–return” 11 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyKRj0IAS9ehIjmAckSZieTZMjs7DLTK4Qln+DFgyJe/SJv/o2TZA8aLWgoqrrp7gpiKQy67peTW1peWV3Lrxc2Nre2d4q7ew0TJZrxOotkpFsBNVwKxesoUPJWrDkNA8mbweh66jcfuTYiUg84jrkf0oESfcEoWun+9uqsWyy5ZXcG8pd4GSlBhlq3+NnpRSwJuUImqTFtz43RT6lGwSSfFDqJ4TFlIzrgbUsVDbnx09mpE3JklR7pR9qWQjJTf06kNDRmHAa2M6Q4NIveVPzPayfYv/BToeIEuWLzRf1EEozI9G/SE5ozlGNLKNPC3krYkGrK0KZTsCF4iy//JY2TslcpV+5OS9XLLI48HMAhHIMH51CFG6hBHRgM4Ale4NWRzrPz5rzPW3NONrMPv+B8fAOdYo1Z M =5 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyKRD0IAS9ehIjmAckSZie9yZDZ2WVmVgghn+DFgyJe/SJv/o2TZA+aWNBQVHXT3RUkgmvjut9ObmV1bX0jv1nY2t7Z3SvuHzR0nCqGdRaLWLUCqlFwiXXDjcBWopBGgcBmMLyZ+s0nVJrH8tGMEvQj2pc85IwaKz3cXZ93iyW37M5AlomXkRJkqHWLX51ezNIIpWGCat323MT4Y6oMZwInhU6qMaFsSPvYtlTSCLU/np06ISdW6ZEwVrakITP198SYRlqPosB2RtQM9KI3Ff/z2qkJL/0xl0lqULL5ojAVxMRk+jfpcYXMiJEllClubyVsQBVlxqZTsCF4iy8vk8ZZ2auUK/fnpepVFkcejuAYTsGDC6jCLdSgDgz68Ayv8OYI58V5dz7mrTknmzmEP3A+fwCb3o1Y M =4 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyqRD0IAS9ehIjmAckSZieTZMjs7DLTK4Qln+DFgyJe/SJv/o2TZA8aLWgoqrrp7gpiKQy67peTW1peWV3Lrxc2Nre2d4q7ew0TJZrxOotkpFsBNVwKxesoUPJWrDkNA8mbweh66jcfuTYiUg84jrkf0oESfcEoWun+9uq0Wyy5ZXcG8pd4GSlBhlq3+NnpRSwJuUImqTFtz43RT6lGwSSfFDqJ4TFlIzrgbUsVDbnx09mpE3JklR7pR9qWQjJTf06kNDRmHAa2M6Q4NIveVPzPayfYv/BToeIEuWLzRf1EEozI9G/SE5ozlGNLKNPC3krYkGrK0KZTsCF4iy//JY2TslcpV+7OStXLLI48HMAhHIMH51CFG6hBHRgM4Ale4NWRzrPz5rzPW3NONrMPv+B8fAOaWo1X M =3 AAAB6nicbVDLSgNBEOz1GeMr6tHLYBA8hd0gUQ9CwIsXIaJ5QLKE2UlvMmR2dpmZFULIJ3jxoIhXv8ibf+Mk2YMmFjQUVd10dwWJ4Nq47rezsrq2vrGZ28pv7+zu7RcODhs6ThXDOotFrFoB1Si4xLrhRmArUUijQGAzGN5M/eYTKs1j+WhGCfoR7UseckaNlR7ursvdQtEtuTOQZeJlpAgZat3CV6cXszRCaZigWrc9NzH+mCrDmcBJvpNqTCgb0j62LZU0Qu2PZ6dOyKlVeiSMlS1pyEz9PTGmkdajKLCdETUDvehNxf+8dmrCS3/MZZIalGy+KEwFMTGZ/k16XCEzYmQJZYrbWwkbUEWZsenkbQje4svLpFEueZVS5f68WL3K4sjBMZzAGXhwAVW4hRrUgUEfnuEV3hzhvDjvzse8dcXJZo7gD5zPH5jWjVY= M =2 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyKRD0IAS9ehIjmAckSZieTZMjs7DLTK4Qln+DFgyJe/SJv/o2TZA+aWNBQVHXT3RXEUhh03W8nt7K6tr6R3yxsbe/s7hX3DxomSjTjdRbJSLcCargUitdRoOStWHMaBpI3g9HN1G8+cW1EpB5xHHM/pAMl+oJRtNLD3bXXLZbcsjsDWSZeRkqQodYtfnV6EUtCrpBJakzbc2P0U6pRMMknhU5ieEzZiA5421JFQ278dHbqhJxYpUf6kbalkMzU3xMpDY0Zh4HtDCkOzaI3Ff/z2gn2L/1UqDhBrth8UT+RBCMy/Zv0hOYM5dgSyrSwtxI2pJoytOkUbAje4svLpHFW9irlyv15qXqVxZGHIziGU/DgAqpwCzWoA4MBPMMrvDnSeXHenY95a87JZg7hD5zPH5dSjVU= M =1 Figure 6: Effect of prediction horizon M on S ( x ) , with N = 5 , a = 0 . 95 , Q = 10 I 2 , and R = I 2 . -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 AAAB7nicbVBNSwMxEJ2tX7V+VT16CRbBU9kVqXoQCl48VrAf0C4lm2bb0GwSkqxQlv4ILx4U8erv8ea/MW33oK0PBh7vzTAzL1KcGev7315hbX1jc6u4XdrZ3ds/KB8etYxMNaFNIrnUnQgbypmgTcsspx2lKU4iTtvR+G7mt5+oNkyKRztRNEzwULCYEWyd1O7pkbwN/H654lf9OdAqCXJSgRyNfvmrN5AkTaiwhGNjuoGvbJhhbRnhdFrqpYYqTMZ4SLuOCpxQE2bzc6fozCkDFEvtSlg0V39PZDgxZpJErjPBdmSWvZn4n9dNbXwdZkyo1FJBFovilCMr0ex3NGCaEssnjmCimbsVkRHWmFiXUMmFECy/vEpaF9WgVq09XFbqN3kcRTiBUziHAK6gDvfQgCYQGMMzvMKbp7wX7937WLQWvHzmGP7A+/wBhG6PBQ== ⇢ = 10 AAAB7XicbVBNSwMxEJ34WetX1aOXYBE8lV2Rqgeh4MVjBfsB7VKyabaNzSZLkhXK0v/gxYMiXv0/3vw3pu0etPXBwOO9GWbmhYngxnreN1pZXVvf2CxsFbd3dvf2SweHTaNSTVmDKqF0OySGCS5Zw3IrWDvRjMShYK1wdDv1W09MG67kgx0nLIjJQPKIU2Kd1Ozqobrxe6WyV/FmwMvEz0kZctR7pa9uX9E0ZtJSQYzp+F5ig4xoy6lgk2I3NSwhdEQGrOOoJDEzQTa7doJPndLHkdKupMUz9fdERmJjxnHoOmNih2bRm4r/eZ3URldBxmWSWibpfFGUCmwVnr6O+1wzasXYEUI1d7diOiSaUOsCKroQ/MWXl0nzvOJXK9X7i3LtOo+jAMdwAmfgwyXU4A7q0AAKj/AMr/CGFHpB7+hj3rqC8pkj+AP0+QMUrY7L ⇢ =1 AAAB73icbVBNSwMxEJ2tX7V+VT16CRbB07IrWvUgFLx4rGA/oF1KNs22odlkTbJCWfonvHhQxKt/x5v/xrTdg7Y+GHi8N8PMvDDhTBvP+3YKK6tr6xvFzdLW9s7uXnn/oKllqghtEMmlaodYU84EbRhmOG0niuI45LQVjm6nfuuJKs2keDDjhAYxHggWMYKNldpdNZQ3nnvRK1c815sBLRM/JxXIUe+Vv7p9SdKYCkM41rrje4kJMqwMI5xOSt1U0wSTER7QjqUCx1QH2ezeCTqxSh9FUtkSBs3U3xMZjrUex6HtjLEZ6kVvKv7ndVITXQUZE0lqqCDzRVHKkZFo+jzqM0WJ4WNLMFHM3orIECtMjI2oZEPwF19eJs0z16+61fvzSu06j6MIR3AMp+DDJdTgDurQAAIcnuEV3pxH58V5dz7mrQUnnzmEP3A+fwD3ZY9B ⇢ =0 . 5 AAAB73icbVBNSwMxEJ2tX7V+VT16CRbB07JbpOpBKHjxWMF+QLuUbJptQ7PJmmSFsvRPePGgiFf/jjf/jWm7B219MPB4b4aZeWHCmTae9+0U1tY3NreK26Wd3b39g/LhUUvLVBHaJJJL1QmxppwJ2jTMcNpJFMVxyGk7HN/O/PYTVZpJ8WAmCQ1iPBQsYgQbK3V6aiRvPLfaL1c815sDrRI/JxXI0eiXv3oDSdKYCkM41rrre4kJMqwMI5xOS71U0wSTMR7SrqUCx1QH2fzeKTqzygBFUtkSBs3V3xMZjrWexKHtjLEZ6WVvJv7ndVMTXQUZE0lqqCCLRVHKkZFo9jwaMEWJ4RNLMFHM3orICCtMjI2oZEPwl19eJa2q69fc2v1FpX6dx1GEEziFc/DhEupwBw1oAgEOz/AKb86j8+K8Ox+L1oKTzxzDHzifP/LZjz4= ⇢ =0 . 2 AAAB8XicbVDLSgMxFL1TX7W+qi7dBIvgqsyItLoQCm5cVrAPbIeSSTNtaCYZkoxQhv6FGxeKuPVv3Pk3ZtpZaOuBkMM593LvPUHMmTau++0U1tY3NreK26Wd3b39g/LhUVvLRBHaIpJL1Q2wppwJ2jLMcNqNFcVRwGknmNxmfueJKs2keDDTmPoRHgkWMoKNlR77aixv3KrreoNyJfszoFXi5aQCOZqD8ld/KEkSUWEIx1r3PDc2foqVYYTTWamfaBpjMsEj2rNU4IhqP51vPENnVhmiUCr7hEFz9XdHiiOtp1FgKyNsxnrZy8T/vF5iwis/ZSJODBVkMShMODISZeejIVOUGD61BBPF7K6IjLHCxNiQSjYEb/nkVdK+qHq1au3+stK4zuMowgmcwjl4UIcG3EETWkBAwDO8wpujnRfn3flYlBacvOcY/sD5/AHR2o+x ⇢ =0 . 001 Figure 7: Effect of MPC weighting Q = ρI 2 on S ( x ) , with N = M = 5 , a = 0 . 95 , and R = I 2 . 12 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees -1 -0.5 0 0.5 1 -1 -0.5 0 0.5 1 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyKRj0IAS+eJKJ5QLKE2ckkGTI7u8z0CmHJJ3jxoIhXv8ibf+Mk2YNGCxqKqm66u4JYCoOu++XklpZXVtfy64WNza3tneLuXsNEiWa8ziIZ6VZADZdC8ToKlLwVa07DQPJmMLqe+s1Hro2I1AOOY+6HdKBEXzCKVrq/vTrrFktu2Z2B/CVeRkqQodYtfnZ6EUtCrpBJakzbc2P0U6pRMMknhU5ieEzZiA5421JFQ278dHbqhBxZpUf6kbalkMzUnxMpDY0Zh4HtDCkOzaI3Ff/z2gn2L/xUqDhBrth8UT+RBCMy/Zv0hOYM5dgSyrSwtxI2pJoytOkUbAje4st/SeOk7FXKlbvTUvUyiyMPB3AIx+DBOVThBmpQBwYDeIIXeHWk8+y8Oe/z1pyTzezDLzgf357ojVo= N =5 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyKRD0IAS+eJKJ5QLKE2UlvMmR2dpmZFULIJ3jxoIhXv8ibf+Mk2YMmFjQUVd10dwWJ4Nq47reTW1ldW9/Ibxa2tnd294r7Bw0dp4phncUiVq2AahRcYt1wI7CVKKRRILAZDG+mfvMJleaxfDSjBP2I9iUPOaPGSg931+fdYsktuzOQZeJlpAQZat3iV6cXszRCaZigWrc9NzH+mCrDmcBJoZNqTCgb0j62LZU0Qu2PZ6dOyIlVeiSMlS1pyEz9PTGmkdajKLCdETUDvehNxf+8dmrCS3/MZZIalGy+KEwFMTGZ/k16XCEzYmQJZYrbWwkbUEWZsekUbAje4svLpHFW9irlyv15qXqVxZGHIziGU/DgAqpwCzWoA4M+PMMrvDnCeXHenY95a87JZg7hD5zPH51kjVk= N =4 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyqRD0IAS+eJKJ5QLKE2ckkGTI7u8z0CmHJJ3jxoIhXv8ibf+Mk2YNGCxqKqm66u4JYCoOu++XklpZXVtfy64WNza3tneLuXsNEiWa8ziIZ6VZADZdC8ToKlLwVa07DQPJmMLqe+s1Hro2I1AOOY+6HdKBEXzCKVrq/vTrtFktu2Z2B/CVeRkqQodYtfnZ6EUtCrpBJakzbc2P0U6pRMMknhU5ieEzZiA5421JFQ278dHbqhBxZpUf6kbalkMzUnxMpDY0Zh4HtDCkOzaI3Ff/z2gn2L/xUqDhBrth8UT+RBCMy/Zv0hOYM5dgSyrSwtxI2pJoytOkUbAje4st/SeOk7FXKlbuzUvUyiyMPB3AIx+DBOVThBmpQBwYDeIIXeHWk8+y8Oe/z1pyTzezDLzgf35vgjVg= N =3 AAAB6nicbVDLSgNBEOz1GeMr6tHLYBA8hd0gUQ9CwIsniWgekCxhdtKbDJmdXWZmhRDyCV48KOLVL/Lm3zhJ9qCJBQ1FVTfdXUEiuDau++2srK6tb2zmtvLbO7t7+4WDw4aOU8WwzmIRq1ZANQousW64EdhKFNIoENgMhjdTv/mESvNYPppRgn5E+5KHnFFjpYe763K3UHRL7gxkmXgZKUKGWrfw1enFLI1QGiao1m3PTYw/pspwJnCS76QaE8qGtI9tSyWNUPvj2akTcmqVHgljZUsaMlN/T4xppPUoCmxnRM1AL3pT8T+vnZrw0h9zmaQGJZsvClNBTEymf5MeV8iMGFlCmeL2VsIGVFFmbDp5G4K3+PIyaZRLXqVUuT8vVq+yOHJwDCdwBh5cQBVuoQZ1YNCHZ3iFN0c4L8678zFvXXGymSP4A+fzB5pcjVc= N =2 AAAB6nicbVDLSgNBEOyNrxhfUY9eBoPgKeyKRD0IAS+eJKJ5QLKE2ckkGTI7u8z0CmHJJ3jxoIhXv8ibf+Mk2YMmFjQUVd10dwWxFAZd99vJrayurW/kNwtb2zu7e8X9g4aJEs14nUUy0q2AGi6F4nUUKHkr1pyGgeTNYHQz9ZtPXBsRqUccx9wP6UCJvmAUrfRwd+11iyW37M5AlomXkRJkqHWLX51exJKQK2SSGtP23Bj9lGoUTPJJoZMYHlM2ogPetlTRkBs/nZ06ISdW6ZF+pG0pJDP190RKQ2PGYWA7Q4pDs+hNxf+8doL9Sz8VKk6QKzZf1E8kwYhM/yY9oTlDObaEMi3srYQNqaYMbToFG4K3+PIyaZyVvUq5cn9eql5lceThCI7hFDy4gCrcQg3qwGAAz/AKb450Xpx352PemnOymUP4A+fzB5jYjVY= N =1 (Infeasible) Figure 8: Effect of prediction horizon N on S ( x ) , with Q = 10 I 2 , R = I 2 , a = 0 . 95 , and M = N . task cannot be reliably accomplished by any single con ventional controller , including a standard MPC design, because the mission inherently inv olves distinct and sometimes conﬂicting control objecti ves across phases, ranging from aggressiv e motion to long-duration stabilisation. T o make the task even more challenging, we impose state and input constraints: X = [ − 0 . 5 , 0 . 5] 2 × [ − π / 3 , π / 3] , U = [ − 10 , 10] 2 . W ithout loss of generality , the collision volume of the robot is ignored here. The designated target that the robot must reach and remain is the point p go = [0 . 5 , 0 . 5] ⊤ , the upper-right corner of the allow able position set, making the dwelling phase particularly demanding due to the tight state constraints. 4.3.1 The Baseline Appr oach A common baseline solution for this type of task is to design two separate controllers, each dedicated to one phase of the operation: a controller that driv es the robot toward the goal (the “go–stay” phase) and another that brings it back to the origin (the “return” phase). These controllers operate independently and are switched sequentially to complete the ov erall mission. T o generate these task-oriented commands, we adopt a reinforcement-learning-like optimal control strategy that pri- oritises goal achiev ement. Let D go ( i ; x ) :=   [ p x ( i ; x ) , p y ( i ; x )] ⊤ − p go   2 (18) denote the squared distance between the predicted robot position at step i and the target point p go . The controller for the “go-stay” phase is giv en by u ∗ ( x ( k )) = { u ∗ (0; x ( k )) , . . . , u ∗ ( H − 1; x ( k )) } = arg min u H − 1 X i =0 γ i D go ( i ; x ( k )) , u go ( k ) = u ∗ (0; x ( k )) , (19) where H = 5 is the prediction horizon and γ = 0 . 9 ∈ (0 , 1) is a discount factor . Similarly , the “return” phase controller is obtained by replacing the goal position p go with the home position p return = [0 , 0] ⊤ in (18) and (19). The resulting control law drives the robot back toward the origin using the same discounted predictiv e criterion, and its ﬁrst control action is taken as the command for the return phase, denoted by u return ( k ) . Thus, under the con ventional two-controller frame work, the robot ﬁrst applies u go ( k ) during the “go–stay” phase and subsequently switches to u return ( k ) for the “return” phase. 13 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees 4.3.2 PS 2 F with Mode Scheduling This dynamic task requirement can also be naturally integrated into the PS 2 F framew ork through Algorithm 2. In this demonstration, we only employ the external command u ext ( k ) = u go ( k ) and omit the return controller u return ( k ) . The nominal MPC employs the quadratic stage cost ℓ ( x, u ) = x ⊤ Qx + u ⊤ Ru with tuning matrices Q = 10 I 3 and R = I 2 , prediction horizon N = 5 , terminal cost V f ( x ) = 0 , and terminal set X f = { 0 3 × 1 } . F or the PS 2 F layer , the scheduling parameters are chosen as M ( k ) = N = 5 for all k ∈ I ≥ 0 , together with a time-varying coefﬁcient, gi ven by a ( k ) = ( 100 , k ∈ I 0: K s − 1 , 0 . 5 , k ∈ I ≥ K s , so that the closed-loop system prioritises safety during the initial phase ( k ∈ I 0: K s − 1 ) and transitions to a stability- oriented behaviour once the system is sufﬁciently close to the desired operating region. This scheduled adjustment enables the PS 2 F mechanism to balance short-term safety requirements with long-term stability guarantees. 4.3.3 Results The state and input responses are shown in Fig. 9. For the baseline approach, the controller is scheduled to switch to the return mode at k = 30 (i.e., at t = 6 s ), which corresponds to the blue curve in the ﬁltered case in Fig. 9. It is worth noting that the ﬁltered case is slower than the baseline because it must keep the heading angle within the admissible safety re gion. This constraint requires additional time for the mobile robot to reorient before proceeding, as illustrated in Fig. 9(a). From Fig. 9, it can also be observed that the proposed method guarantees that the mobile robot safely completes the entire “go–stay–return” task, regardless of the choice of K s . Fig. 9(c) shows the value function of the nominal MPC. As expected, it is nondecreasing before K s because, during the “go–stay” phase, the robot is activ ely moving away from the origin, causing the value function to increase (or at least not decrease). After K s , according to Theorem 2, the value function becomes decreasing, reﬂecting the transition to the stability-oriented phase of the controller . T o provide a more intuitiv e illustration, Fig. 10 displays the robot’ s position trajectories, where the triangular mark- ers indicate the instantaneous heading angle. In Fig. 10(a), the baseline controller violates the heading-angle safety constraint when attempting to initiate motion near k = 0 or to return near k = 30 , resulting in unsafe behaviour . In contrast, Fig. 10(b) shows that the proposed PS 2 F method successfully maintains the robot within the admissible region while following the same switching schedule, demonstrating the effecti veness of the safety ﬁlter in enforcing constraint satisfaction throughout the manoeuvre. 5 Conclusions In this paper, we have dev eloped a uniﬁed predictive control frame work for achieving safe and stable control under potentially unsafe or unstable external commands. Unlike most existing ﬁlter or safety-layer designs, the proposed Pr edictive Safety–Stability Filter (PS 2 F) provides a principled yet ﬂexible mechanism that guarantees both safety and stability through a cascaded optimisation structure. By emplo ying a nominal MPC layer as a copilot , PS 2 F inherits the theoretical guarantees of recursive feasibility and asymptotic stability while still allowing the incorporation of arbi- trary goal-oriented external commands. Moreover , a time-v arying parameterisation enables smooth mode scheduling, allowing the controller to prioritise safety during exploration and progressiv ely enforce stability during exploitation. Numerical studies have demonstrated the effecti veness and versatility of the proposed framework across div erse operat- ing scenarios. Future work will focus on extending the frame work to uncertain, disturbed, or stochastic en vironments, and on implementing PS 2 F on real robotic platforms to further validate its practical applicability . A ppendix The appendix collects the detailed proofs: Appendix A contains the proofs of the propositions related to the second OCP , P f ,M ( x ) , while Appendix B presents the proofs concerning the closed-loop behaviour of the o verall system. A Proof of Pr opositions Proof of Proposition 2. Since x ∈ X N , the nominal MPC problem P N ( x ) is feasible, and hence there exist optimal sequences v ∗ ( x ) and z ∗ ( x ) . Consider the truncated sequences formed by taking the ﬁrst M +1 elements of z ∗ ( x ) and 14 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees 0 2 4 6 8 10 12 14 Time (s) 0 0.5 AAAB7XicbVBNSwMxEJ2tX7V+VT16CRbBU9ktpfUiFLwIXirYD2iXkk2zbWw2WZKsUJb+By8eFPHq//HmvzFt96CtDwYe780wMy+IOdPGdb+d3Mbm1vZOfrewt39weFQ8PmlrmShCW0RyqboB1pQzQVuGGU67saI4CjjtBJObud95okozKR7MNKZ+hEeChYxgY6X23UBfV9xBseSW3QXQOvEyUoIMzUHxqz+UJImoMIRjrXueGxs/xcowwums0E80jTGZ4BHtWSpwRLWfLq6doQurDFEolS1h0EL9PZHiSOtpFNjOCJuxXvXm4n9eLzHhlZ8yESeGCrJcFCYcGYnmr6MhU5QYPrUEE8XsrYiMscLE2IAKNgRv9eV10q6UvVq5dl8tNapZHHk4g3O4BA/q0IBbaEILCDzCM7zCmyOdF+fd+Vi25pxs5hT+wPn8AY5Sjm8= K s = 20 AAAB7XicbVBNSwMxEJ2tX7V+tOrRS7AInsqulupFKHgRvFSwH9AuJZtm29hssiRZoSz9D148KOLV/+PNf2Pa7kFbHww83pthZl4Qc6aN6347ubX1jc2t/HZhZ3dvv1g6OGxpmShCm0RyqToB1pQzQZuGGU47saI4CjhtB+Obmd9+okozKR7MJKZ+hIeChYxgY6XWXV9fX7j9UtmtuHOgVeJlpAwZGv3SV28gSRJRYQjHWnc9NzZ+ipVhhNNpoZdoGmMyxkPatVTgiGo/nV87RadWGaBQKlvCoLn6eyLFkdaTKLCdETYjvezNxP+8bmLCKz9lIk4MFWSxKEw4MhLNXkcDpigxfGIJJorZWxEZYYWJsQEVbAje8surpHVe8WqV2n21XK9mceThGE7gDDy4hDrcQgOaQOARnuEV3hzpvDjvzseiNedkM0fwB87nD4/XjnA= K s = 30 Constraints Proposed , Proposed , Baseline Proposed , AAAB7XicbVBNSwMxEJ2tX7V+VT16CRbBU9mVUr0IBS+Clwr2A9qlZNNsG5tNliQrlKX/wYsHRbz6f7z5b0y3e9DWBwOP92aYmRfEnGnjut9OYW19Y3OruF3a2d3bPygfHrW1TBShLSK5VN0Aa8qZoC3DDKfdWFEcBZx2gsnN3O88UaWZFA9mGlM/wiPBQkawsVL7bqCva+6gXHGrbga0SrycVCBHc1D+6g8lSSIqDOFY657nxsZPsTKMcDor9RNNY0wmeER7lgocUe2n2bUzdGaVIQqlsiUMytTfEymOtJ5Gge2MsBnrZW8u/uf1EhNe+SkTcWKoIItFYcKRkWj+OhoyRYnhU0swUczeisgYK0yMDahkQ/CWX14l7YuqV6/W72uVRi2PowgncArn4MElNOAWmtACAo/wDK/w5kjnxXl3PhatBSefOYY/cD5/AJFcjnE= K s = 40 AAAB7nicbVBNS8NAEJ34WetX1aOXxSJ4KomU6rHgxWMF+wFtKJvtpF2y2YTdjVBCf4QXD4p49fd489+4bXPQ1gcDj/dmmJkXpIJr47rfzsbm1vbObmmvvH9weHRcOTnt6CRTDNssEYnqBVSj4BLbhhuBvVQhjQOB3SC6m/vdJ1SaJ/LRTFP0YzqWPOSMGit1o8EYSd0dVqpuzV2ArBOvIFUo0BpWvgajhGUxSsME1brvuanxc6oMZwJn5UGmMaUsomPsWyppjNrPF+fOyKVVRiRMlC1pyEL9PZHTWOtpHNjOmJqJXvXm4n9ePzPhrZ9zmWYGJVsuCjNBTELmv5MRV8iMmFpCmeL2VsImVFFmbEJlG4K3+vI66VzXvEat8VCvNutFHCU4hwu4Ag9uoAn30II2MIjgGV7hzUmdF+fd+Vi2bjjFzBn8gfP5Az/QjtQ= k  40 AAAB83icbVDLSgNBEOyNrxhfUY9eBoPgKezGED14CHjxGME8ILuE2UlvMmT2wcysEJb8hhcPinj1Z7z5N06SPWhiQUNR1U13l58IrrRtf1uFjc2t7Z3ibmlv/+DwqHx80lFxKhm2WSxi2fOpQsEjbGuuBfYSiTT0BXb9yd3c7z6hVDyOHvU0QS+ko4gHnFFtJLdmE1cgmdySK3tQrthVewGyTpycVCBHa1D+cocxS0OMNBNUqb5jJ9rLqNScCZyV3FRhQtmEjrBvaERDVF62uHlGLowyJEEsTUWaLNTfExkNlZqGvukMqR6rVW8u/uf1Ux3ceBmPklRjxJaLglQQHZN5AGTIJTItpoZQJrm5lbAxlZRpE1PJhOCsvrxOOrWq06g2HuqVZj2PowhncA6X4MA1NOEeWtAGBgk8wyu8Wan1Yr1bH8vWgpXPnMIfWJ8/WHiP6A== 20  k< 30 AAAB83icbVA9SwNBEJ2LXzF+RS1tFoNgFe40RAuLgI1lBGMCuSPsbeaSJXt7x+6eEEL+ho2FIrb+GTv/jZvkCk18MPB4b4aZeWEquDau++0U1tY3NreK26Wd3b39g/Lh0aNOMsWwxRKRqE5INQousWW4EdhJFdI4FNgOR7czv/2ESvNEPphxikFMB5JHnFFjJf/SJb5AMrohNbdXrrhVdw6ySrycVCBHs1f+8vsJy2KUhgmqdddzUxNMqDKcCZyW/ExjStmIDrBrqaQx6mAyv3lKzqzSJ1GibElD5urviQmNtR7Hoe2MqRnqZW8m/ud1MxNdBxMu08ygZItFUSaIScgsANLnCpkRY0soU9zeStiQKsqMjalkQ/CWX14ljxdVr16t39cqjVoeRxFO4BTOwYMraMAdNKEFDFJ4hld4czLnxXl3PhatBSefOYY/cD5/AFuMj+o= 30  k< 40 0 2 4 6 8 10 12 14 Time (s) 0 0.5 0 2 4 6 8 10 12 14 Time (s) -30 0 60 (a) 0 2 4 6 8 10 12 14 Time (s) -4 -2 0 2 4 0 2 4 6 8 10 12 14 Time (s) -10 0 10 (b) 0 2 4 6 8 10 12 14 Time (s) 0 20 40 (c) Figure 9: Closed-loop ev olution of the robot system under the baseline controller and the proposed PS 2 F method. (a) States; (b) Control inputs; (c) V alue function. the ﬁrst M elements of v ∗ ( x ) , i.e., ˜ x ( x ) := { z ∗ (0; x ) , . . . , z ∗ ( M ; x ) } , ˜ u ( x ) := { v ∗ (0; x ) , . . . , v ∗ ( M − 1; x ) } . (20) It is straightforward to verify that ˜ x ( x ) and ˜ u ( x ) satisfy (11a) and (11b); therefore, P f ,M ( x ) is feasible. This completes the proof. ■ Proof of Pr oposition 3. The ﬁrst property is proved by contradiction. Assume that L ( x ∗ 0: M − 1 ( x ) , u ∗ ( x )) < L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) . 15 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees -0.25 0 0.25 0.5 0 0.25 0.5 1 2 3 4 (a) -0.25 0 0.25 0.5 0 0.25 0.5 1 2 3 4 5 (b) Figure 10: Comparison of robot trajectories under the baseline controller and the proposed PS 2 F method when both are scheduled to return at k = 30 . A yellow robot marker denotes that the state violates the safety constraints. (a) Baseline. (b) Proposed. Consider the extended sequences constructed by appending the tail of z ∗ ( x ) and v ∗ ( x ) to x ∗ ( x ) and u ∗ ( x ) , respec- tiv ely: ˜ z ( x ) := { x ∗ (0; x ) , . . . , x ∗ ( M ; x ) , z ∗ ( M + 1; x ) , . . . , z ∗ ( N ; x ) } ˜ v ( x ) := { u ∗ (0; x ) , . . . , u ∗ ( M − 1; x ) , v ∗ ( M ; x ) , . . . , v ∗ ( N − 1; x ) } Since z ∗ ( M ; x ) = x ∗ ( M ; x ) from (11b), the sequence ˜ z ( x ) is the state trajectory induced by the input sequence ˜ v ( x ) . Both are feasible for P N ( x ) , and moreov er, V N ( x, ˜ v ( x )) = M − 1 X i =0 ℓ ( x ∗ ( i ; x ) , u ∗ ( i ; x )) + N − 1 X i = M ℓ ( z ∗ ( i ; x ) , v ∗ ( i ; x )) + V f ( z ∗ ( N ; x )) = L ( x ∗ 0: M − 1 ( x ) , u ∗ ( x )) + V N ( x, v ∗ ( x )) − L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) < V N ( x, v ∗ ( x )) = V ∗ N ( x ) which contradicts the optimality of v ∗ ( x ) for P N ( x ) . Hence, L ( x ∗ 0: M − 1 ( x ) , u ∗ ( x )) ≥ L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) . By noting (11a), equality follows: L ( x ∗ 0: M − 1 ( x ) , u ∗ ( x )) = L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) . W ith property (a) established, we ha ve V N ( x, ˜ v ( x )) = V N ( x, v ∗ ( x )) . If P N ( x ) is strictly conv ex, this implies ˜ v ( x ) = v ∗ ( x ) , and thus u = u ∗ (0; x ) = v ∗ (0; x ) . This completes the proof. ■ Proof of Pr oposition 4. Consider a = a 2 . For any element u (0; x ) ∈ S a 2 ( x ) , there exists a control sequence u ( x ) = { u (0; x ) , u (1; x ) , . . . , u ( M − 1; x ) } ∈ U a 2 M ( x ) such that the corresponding state trajectory satisﬁes the inequality L ( x 0: M − 1 ( x ) , u ( x )) − L ( z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )) ≤ a 2 ℓ ( x (0; x ) , u (0; x )) ≤ a 1 ℓ ( x (0; x ) , u (0; x )) Hence, u ( x ) ∈ U a 1 M ( x ) , which implies that u (0; x ) ∈ S a 1 ( x ) . Therefore, S a 2 ( x ) ⊆ S a 1 ( x ) . This completes the proof. ■ 16 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees Proof of Proposition 5. For M = 1 , the terminal equality constraint (11b) reduces to x (1; x ) = z ∗ (1; x ) . By the system dynamics, this is equiv alent to f  x, u ∗ (0; x )  = f  x, v ∗ (0; x )  . Since the map u 7→ f ( x, u ) is injectiv e for the giv en state x , the above equality implies u ∗ (0; x ) = v ∗ (0; x ) . This completes the proof. ■ Proof of Pr oposition 6. Consider M = i . For an y element u (0; x ) ∈ S i ( x ) , there exists a control sequence u i ( x ) = { u (0; x ) , u (1; x ) , . . . , u ( i − 1; x ) } ∈ U a i ( x ) such that the following hold: L ( x 0: i − 1 ( x ) , u i ( x )) − L ( z ∗ 0: i − 1 ( x ) , v ∗ 0: i − 1 ( x )) − aℓ ( x (0; x ) , u (0; x )) ≤ 0 , (21a) x ( i ; x ) = z ∗ ( i ; x ) . (21b) Using (21b), we can extend the control sequence u i ( x ) by appending the optimal input v ∗ ( i ; x ) to obtain ˜ u i +1 ( x ) := { u (0; x ) , u (1; x ) , . . . , u ( i − 1; x ) , v ∗ ( i ; x ) } . The corresponding state sequence is ˜ x ( x ) := { x (0; x ) , . . . , x ( i − 1; x ) , z ∗ ( i ; x ) , z ∗ ( i + 1; x ) } . The constructed pair ( ˜ x ( x ) , ˜ u i +1 ( x )) satisﬁes both (21a) and (21b) with i replaced by i + 1 . Hence, ˜ u i +1 ( x ) ∈ U a i +1 ( x ) , which implies that u (0; x ) ∈ S i +1 ( x ) . Therefore, S i ( x ) ⊆ S i +1 ( x ) for all i ∈ I 1: N − 1 . This completes the proof. ■ Proof of Proposition 7. Since the safety constraints in both OCPs, as well as the terminal constraint of the ﬁrst OCP , are inactiv e, the nominal MPC problem coincides with the inﬁnite-horizon LQR problem. Hence, the value function of the nominal MPC is V ∗ N ( x ) = x ⊤ P x , and the optimal state and input trajectories are giv en by z ∗ ( i ; x ) = A i LQR x, i = 0 , 1 , . . . , N , v ∗ ( i ; x ) = − K LQR z ∗ ( i ; x ) , (22) where K LQR := ( R + B ⊤ P B ) − 1 B ⊤ P A qnd A LQR := A − B K LQR . W ith (22), the cumulative stage cost of the nominal optimal trajectory ov er the ﬁrst M steps is L  z ∗ 0: M − 1 ( x ) , v ∗ 0: M − 1 ( x )  = M − 1 X i =0 ℓ ( z ∗ ( i ; x ) , v ∗ ( i ; x )) + V f ( z ∗ ( M ; x )) − V f ( z ∗ ( M ; x )) = z ∗ (0; x ) ⊤ P z ∗ (0; x ) − z ∗ ( M ; x ) ⊤ P z ∗ ( M ; x ) = x ⊤  P − A M ⊤ LQR P A M LQR  x. (23) Next, we deriv e the lifted form of the dynamics and rewrite the conditions (11a) and (11b) accordingly . Over a prediction horizon of length M , deﬁne the stacked state and input vectors x :=    x (0; x ) . . . x ( M ; x )    ∈ R ( M +1) n , u :=    u (0; x ) . . . u ( M − 1; x )    ∈ R M m . For notational simplicity , we use the same symbols x and u as in the sequence representation of the second OCP , P f ,M ( x ) ; in this conte xt, ho wever , the y denote the stacked state and input v ectors. The system dynamics x ( i + 1; x ) = Ax ( i ; x ) + B u ( i ; x ) imply the lifted (af ﬁne) dynamics x = Φ M x + Γ M u , (24) with Φ M ∈ R ( M +1) n × n and Γ M ∈ R ( M +1) n × M m , giv en by Φ M :=       I n A A 2 . . . A M       , Γ M :=       0 n × m 0 n × m · · · 0 n × m B 0 n × m · · · 0 n × m AB B · · · 0 n × m . . . . . . . . . . . . A M − 1 B A M − 2 B · · · B       . 17 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees In particular , the terminal state satisﬁes x ( M ; x ) = e M x = e M Φ M x + e M Γ M u , where e M := [0 n × n , · · · , 0 n × n , I n ] ∈ R n × ( M +1) n selects the last state component. The terminal constraint (11b) can be equiv alently expressed as A eq u = b eq x (25) where A eq := e M Γ M and b eq := A M LQR − e M Φ M . T o express the inequality (11a) in compact lifted form, we introduce the block-diagonal matrices ˜ Q := diag( Q, . . . , Q, 0 n × n ) ∈ R ( M +1) n × ( M +1) n , ˜ R := diag( R , . . . , R ) ∈ R M m × M m , so that the cumulativ e stage cost over the ﬁrst M steps can be written as L ( x 0: M − 1 ( x ) , u ( x )) = x ⊤ ˜ Q x + u ⊤ ˜ R u . Using the lifted dynamics (24), we obtain L ( x 0: M − 1 ( x ) , u ( x )) = (Φ M x + Γ M u ) ⊤ ˜ Q (Φ M x + Γ M u ) + u ⊤ ˜ R u = x ⊤ Φ ⊤ M ˜ Q Φ M x + 2 x ⊤ Φ ⊤ M ˜ Q Γ M u + u ⊤  Γ ⊤ M ˜ Q Γ M + ˜ R  u . (26) Similarly , the ﬁrst-stage cost can be written as ℓ ( x (0; x ) , u (0; x )) = x (0; x ) ⊤ Q x (0; x ) + u (0; x ) ⊤ R u (0; x ) = x ⊤ Q x + u ⊤ e ⊤ 1 Re 1 u , (27) where e 1 := [ I m , 0 m × m , · · · , 0 m × m ] ∈ R m × ( M m ) selects the ﬁrst input block from u . Substituting (23), (26), and (27) into (11a) yields u ⊤ H u + 2 x ⊤ F u + x ⊤ Gx ≤ 0 , (28) where H := Γ ⊤ M ˜ Q Γ M + ˜ R − a e ⊤ 1 Re 1 , F := Φ ⊤ M ˜ Q Γ M , G := Φ ⊤ M ˜ Q Φ M −  P − A M ⊤ LQR P A M LQR  − aQ. T ogether with (25) and (28), this establishes the lifted quadratic representation of the PS 2 F constraints and completes the proof. ■ B Proof of Theor ems Proof of Theorem 1. T o prove property (a), we ﬁrst establish recursiv e feasibility . At time step k , suppose that x ( k ) ∈ X N . Then, the nominal MPC problem P N ( x ( k )) is feasible, and the corresponding optimal sequences v ∗ ( x ( k )) and z ∗ ( x ( k )) exist, as deﬁned in (7) and (8). Since z ∗ ( N ; x ( k )) ∈ X f , Assumption 3 ensures the existence of a control input ˜ v ( N ; x ( k )) ∈ U such that the successor state ˜ z ( N +1; x ( k )) := f  z ∗ ( N ; x ( k )) , ˜ v ( N ; x ( k ))  (29) satisﬁes ˜ z ( N +1; x ( k )) ∈ X f and V f  ˜ z ( N +1; x ( k ))  − V f  z ∗ ( N ; x ( k ))  ≤ − ℓ  z ∗ ( N ; x ( k )) , ˜ v ( N ; x ( k ))  . (30) By Proposition 2, the ﬁlter problem P f ,M ( x ( k )) is also feasible under the same condition, and its optimal sequences u ∗ ( x ( k )) and x ∗ ( x ( k )) exist as gi ven in (13) and (14). At time step k + 1 , noting that u ( k ) = u ∗ (0; x ( k )) , the successor state satisﬁes x ( k + 1) = f ( x ( k ) , u ( k )) = x ∗ (1; x ( k )) . T o establish recursi ve feasibility , consider the extended sequences constructed by shifting the optimal 18 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees Assume AAACNXicbZDLSgMxFIYz3qr11iq4cRNshQpSZmrt1J3gxpVUsFpoS0kymTY0cyHJiCXOy7jVvc/iwp249RXM1C68HQj8/Occzp8Px5xJZdsv1tz8wuJSbnklv7q2vrFZKG5dyygRhLZJxCPRwUhSzkLaVkxx2okFRQHm9AaPz7L+zS0VkkXhlZrEtB+gYch8RpAy1qCwU+4FSI0w1q10oC/Syl1lfHBQHhRKdtWeFvwrnJkogVm1BkUr1/MikgQ0VIQjKbuOHau+RkIxwmma7yWSxoiM0ZB2jQxRQGVfTz+Qwn3jeNCPhHmhglP3+4ZGgZSTAB/iwAxngeXvdmb+1+smym/2NQvjRNGQfN3yEw5VBDMg0GOCEsUnRiAimIkLyQgJRJTBloc9j/qG7TSS9oapFkOc6gyFazfqh3bVOa7bTdeIo6brNNzUoHN+g/orrmtVp1FtXNZKpycziMtgF+yBCnCAC07BOWiBNiDgHjyAR/BkPVuv1pv1/jU6Z812tsGPsj4+AXT3pVs= P N ( x ( k )) is feasible Successor state is feasible AAAB/3icbVDLSsNAFJ3UV62vqODGzWARWpCSiFTdFdy4ESrYB7QhTKaTduhkEmYmYolZ+CtuXCji1t9w5984abPQ1gMDh3Pu5Z45XsSoVJb1bRSWlldW14rrpY3Nre0dc3evLcNYYNLCIQtF10OSMMpJS1HFSDcSBAUeIx1vfJX5nXsiJA35nZpExAnQkFOfYqS05JoH/QCpkeclzdRN/JObtPJQGVerrlm2atYUcJHYOSmDHE3X/OoPQhwHhCvMkJQ924qUkyChKGYkLfVjSSKEx2hIeppyFBDpJNP8KTzWygD6odCPKzhVf28kKJByEnh6Mksr571M/M/rxcq/cBLKo1gRjmeH/JhBFcKsDDiggmDFJpogLKjOCvEICYSVrqykS7Dnv7xI2qc1u16r356VG5d5HUVwCI5ABdjgHDTANWiCFsDgETyDV/BmPBkvxrvxMRstGPnOPvgD4/MH34mVVg== P f, M ( x ( k )) AAAB+3icbVBNSwMxEJ31s9avtR69BIsgCGVXpOqt4MVjBfsB7VKyabYNzSZLklXL0r/ixYMiXv0j3vw3pu0etPXBwOO9GWbmhQln2njet7Oyura+sVnYKm7v7O7tuwelppapIrRBJJeqHWJNORO0YZjhtJ0oiuOQ01Y4upn6rQeqNJPi3owTGsR4IFjECDZW6rmlEepyGhmslHxEI3SG/J5b9ireDGiZ+DkpQ456z/3q9iVJYyoM4Vjrju8lJsiwMoxwOil2U00TTEZ4QDuWChxTHWSz2yfoxCp9FEllSxg0U39PZDjWehyHtjPGZqgXvan4n9dJTXQVZEwkqaGCzBdFKUdGomkQqM8UJYaPLcFEMXsrIkOsMDE2rqINwV98eZk0zyt+tVK9uyjXrvM4CnAEx3AKPlxCDW6hDg0g8ATP8ApvzsR5cd6dj3nripPPHMIfOJ8/Q12TRQ== k k +1 Sequence (A.1) Sequence (B.3) is feasible AAAB/XicbVDLSgMxFL1TX7W+xsfOTbAILUKZEam6K7hxJRXsA9phyKSZNjTzIMmIdSj+ihsXirj1P9z5N2baLrT1QOBwzr3ck+PFnEllWd9Gbml5ZXUtv17Y2Nza3jF395oySgShDRLxSLQ9LClnIW0opjhtx4LiwOO05Q2vMr91T4VkUXinRjF1AtwPmc8IVlpyzYNugNXA89L62L0pPZSGJ3a57JpFq2JNgBaJPSNFmKHuml/dXkSSgIaKcCxlx7Zi5aRYKEY4HRe6iaQxJkPcpx1NQxxQ6aST9GN0rJUe8iOhX6jQRP29keJAylHg6cksq5z3MvE/r5Mo/8JJWRgnioZkeshPOFIRyqpAPSYoUXykCSaC6ayIDLDAROnCCroEe/7Li6R5WrGrlertWbF2OasjD4dwBCWw4RxqcA11aACBR3iGV3gznowX4934mI7mjNnOPvyB8fkDuu2UFQ== P N ( x ( k + 1)) AAAB/HicbZDLSgMxFIbP1Futt9Eu3QSL0KKUGZGqC6HgxmUFe4F2KJk004ZmLiQZcRjqq7hxoYhbH8Sdb2PazkJbDyR8/P855OR3I86ksqxvI7eyura+kd8sbG3v7O6Z+wctGcaC0CYJeSg6LpaUs4A2FVOcdiJBse9y2nbHN1O//UCFZGFwr5KIOj4eBsxjBCst9c3iY3l8YleuvbKGymmsr0rfLFlVa1ZoGewMSpBVo29+9QYhiX0aKMKxlF3bipSTYqEY4XRS6MWSRpiM8ZB2NQbYp9JJZ8tP0LFWBsgLhT6BQjP190SKfSkT39WdPlYjuehNxf+8bqy8SydlQRQrGpD5Q17MkQrRNAk0YIISxRMNmAimd0VkhAUmSudV0CHYi19ehtZZ1a5Va3fnpfpVFkceDuEIymDDBdThFhrQBAIJPMMrvBlPxovxbnzMW3NGNlOEP2V8/gAmepKA x ( k + 1) = f ( x ( k ) ,u ( k )) Figure 11: Schematic illustration of the recursiv e feasibility analysis. solution of P f ,M ( x ( k )) one step forward and appending the terminal element of the nominal MPC trajectory . Speciﬁ- cally , deﬁne ˜ v ( x ( k +1)) :=  u ∗ (1; x ( k )) , . . . , u ∗ ( M − 1; x ( k )) , v ∗ ( M ; x ( k )) , . . . , v ∗ ( N − 1; x ( k )) , ˜ v ( N ; x ( k ))  , ˜ z ( x ( k +1)) :=  x ∗ (1; x ( k )) , . . . , x ∗ ( M ; x ( k )) , z ∗ ( M +1; x ( k )) , . . . , z ∗ ( N ; x ( k )) , ˜ z ( N +1; x ( k ))  . (31) Since z ∗ ( M ; x ( k )) = x ∗ ( M ; x ( k )) by (11b), the sequence ˜ z ( x ( k + 1)) is precisely the state trajectory induced by the control sequence ˜ v ( x ( k + 1)) . Therefore, the constructed pair ( ˜ z ( x ( k + 1)) , ˜ v ( x ( k +1))) is feasible for the nominal MPC problem P N ( x ( k + 1)) . By induction, if x (0) ∈ X N , then P N ( x (0)) is feasible at the initial step. From the construction above, feasibility of P N ( x ( k )) at time k implies that both P f ,M ( x ( k +1)) and P N ( x ( k +1)) remain feasible. Hence, P N ( x ( k )) and P f ,M ( x ( k )) are feasible for all k ∈ I ≥ 0 . The overall recursiv e feasibility process is illustrated in Fig. 11. The feasibility of these optimisation problems directly ensures that the state and input constraints are satisﬁed at e very time step. This completes the proof of property (a). T o prove property (b), we employ the optimal value function V ∗ N ( x ( k )) of the nominal MPC problem as a L yapunov function candidate. At time step k +1 , the value of the L yapunov candidate must be no greater than the cost ev aluated along the feasible sequences deﬁned in (31). Hence, V ∗ N ( x ( k + 1)) ≤ V N ( x ( k + 1) , ˜ v ( x ( k +1))) = M − 1 X i =0 ℓ ( x ∗ ( i ; x ( k ))) , u ∗ ( i ; x ( k ))) − ℓ ( x ∗ (0; x ( k )) , u ∗ (0; x ( k ))) + N − 1 X i = M ℓ ( z ∗ ( i ; x ( k )) , v ∗ ( i ; x ( k ))) + ℓ ( z ∗ ( N ; x ( k )) , ˜ v ( N ; x ( k ))) + V f ( ˜ z ( N + 1; x ( k ))) (32) Noting (11a) and (30), it follows that V ∗ N ( x ( k + 1)) ≤ L ( z ∗ 0: M − 1 ( x ( k )) , v ∗ 0: M − 1 ( x ( k ))) − (1 − a ) ℓ ( x ∗ (0; x ( k )) , u ∗ (0; x ( k ))) + N − 1 X i = M ℓ ( z ∗ ( i ; x ( k )) , v ∗ ( i ; x ( k ))) + V f ( z ∗ ( N ; x ( k ))) = V ∗ N ( x ( k )) − (1 − a ) ℓ ( x ∗ (0; x ( k )) , u ∗ (0; x ( k ))) ≤ V ∗ N ( x ( k )) − (1 − a ) α 1 ( | x ( k ) | ) (33) where α 1 ( · ) is a K ∞ function from Assumption 3. By Assumption 3 and Proposition 1, the optimal value function satisﬁes α 1 ( | x ( k ) | ) ≤ V ∗ N ( x ( k )) ≤ β ( | x ( k ) | ) . (34) T ogether with the decrease condition in (33), the value function of the nominal MPC, V ∗ N ( x ( k )) , serves as a valid L yapunov function for the closed-loop system, ensuring asymptotic stability of the origin. This completes the proof of property (b). ■ Proof of Theor em 2. Property (a) can be established by constructing feasible sequences in the same manner as in the proof of Theorem 1, with the ﬁxed horizon M replaced by the time-varying horizon M ( k ) . Since sup k ≥ K s a ( k ) < 1 , there exists a constant ¯ a ∈ (0 , 1] such that a ( k ) ≤ 1 − ¯ a for all k ∈ I ≥ K s . It then follows that V ∗ N ( x ( k +1)) ≤ V ∗ N ( x ( k )) −  1 − a ( k )  α 1  | x ( k ) |  ≤ V ∗ N ( x ( k )) − ¯ a α 1  | x ( k ) |  , ∀ k ∈ I ≥ K s , (35) which ensures monotonic decrease of V ∗ N ( x ( k )) and hence prov es property (b). ■ 19 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees References [1] L. He wing, K. P . W abersich, M. Menner , and M. N. Zeilinger, “Learning-based model predicti ve control: T o ward safe learning in control, ” Annual Revie w of Contr ol, Robotics, and Autonomous Systems , vol. 3, no. 1, pp. 269– 296, 2020. [2] E. Kaufmann, L. Bauersfeld, A. Loquercio, M. M ¨ uller , V . Koltun, and D. Scaramuzza, “Champion-level drone racing using deep reinforcement learning, ” Nature , v ol. 620, no. 7976, pp. 982–987, 2023. [3] Y . Song, A. Romero, M. M ¨ uller , V . K oltun, and D. Scaramuzza, “Reaching the limit in autonomous racing: Optimal control versus reinforcement learning, ” Science Robotics , vol. 8, no. 82, p. eadg1462, 2023. [4] J. Berberich, J. K ¨ ohler , M. A. M ¨ uller , and F . Allg ¨ ower , “Data-driv en model predictive control with stability and robustness guarantees, ” IEEE T ransactions on A utomatic Contr ol , vol. 66, no. 4, pp. 1702–1717, 2020. [5] M. Fazlyab, M. Morari, and G. J. Pappas, “Safety veriﬁcation and robustness analysis of neural networks via quadratic constraints and semideﬁnite programming, ” IEEE T ransactions on Automatic Contr ol , vol. 67, no. 1, pp. 1–15, 2020. [6] Y . Y an, X.-F . W ang, B. J. Marshall, C. Liu, J. Y ang, and W .-H. Chen, “Survi ving disturbances: A predictiv e control framew ork with guaranteed safety , ” Automatica , vol. 158, p. 111238, 2023. [7] K.-C. Hsu, H. Hu, and J. F . Fisac, “The safety ﬁlter: A uniﬁed view of safety-critical control in autonomous systems, ” Annual Review of Contr ol, Robotics, and Autonomous Systems , v ol. 7, 2023. [8] K. P . W abersich, A. J. T aylor , J. J. Choi, K. Sreenath, C. J. T omlin, A. D. Ames, and M. N. Zeilinger , “Data- driv en safety ﬁlters: Hamilton-jacobi reachability , control barrier functions, and predicti ve methods for uncertain systems, ” IEEE Control Systems Ma gazine , vol. 43, no. 5, pp. 137–177, 2023. [9] A. D. Ames, X. Xu, J. W . Grizzle, and P . T ab uada, “Control barrier function based quadratic programs for safety critical systems, ” IEEE T ransactions on Automatic Contr ol , v ol. 62, no. 8, pp. 3861–3876, 2016. [10] K. P . W abersich and M. N. Zeilinger , “Predicti ve control barrier functions: Enhanced safety mechanisms for learning-based control, ” IEEE T ransactions on Automatic Contr ol , v ol. 68, no. 5, pp. 2638–2651, 2022. [11] S. Ha, J. Lee, M. v an de Panne, Z. Xie, W . Y u, and M. Khadiv , “Learning-based legged locomotion: State of the art and future perspectiv es, ” The International Journal of Robotics Resear ch , vol. 44, no. 8, pp. 1396–1427, 2025. [12] B. T earle, K. P . W abersich, A. Carron, and M. N. Zeilinger, “ A predictive safety ﬁlter for learning-based racing control, ” IEEE Robotics and Automation Letters , v ol. 6, no. 4, pp. 7635–7642, 2021. [13] M. Jankovic, “Robust control barrier functions for constrained stabilization of nonlinear systems, ” Automatica , vol. 96, pp. 359–367, 2018. [14] W . S. Cortez and D. V . Dimarogonas, “On compatibility and region of attraction for safe, stabilizing control laws, ” IEEE T ransactions on A utomatic Contr ol , vol. 67, no. 9, pp. 4924–4931, 2022. [15] V . Zinage and E. Bakolas, “Univ ersal barrier functions for safety and stability of constrained nonlinear systems, ” arXiv pr eprint arXiv:2511.01067 , 2025. [16] E. Milios, K. P . W abersich, F . Berkel, and L. Schwenkel, “Stability mechanisms for predictiv e safety ﬁlters, ” in IEEE Confer ence on Decision and Contr ol , 2024, pp. 2409–2416. [17] A. Didier, A. Zanelli, K. P . W abersich, and M. N. Zeilinger , “Predictiv e stability ﬁlters for nonlinear dynamical systems affected by disturbances, ” IF AC-P apersOnLine , v ol. 58, no. 18, pp. 200–207, 2024. [18] M. Rickert, A. Sieverling, and O. Brock, “Balancing exploration and exploitation in sampling-based motion planning, ” IEEE T ransactions on Robotics , vol. 30, no. 6, pp. 1305–1317, 2014. [19] M. Marcano, S. D ´ ıaz, J. P ´ erez, and E. Irigoyen, “ A review of shared control for automated vehicles: Theory and applications, ” IEEE T ransactions on Human-Machine Systems , v ol. 50, no. 6, pp. 475–491, 2020. [20] M. Selvaggio, M. Cognetti, S. Nikolaidis, S. Iv aldi, and B. Siciliano, “ Autonomy in physical human-robot inter- action: A brief survey , ” IEEE Robotics and A utomation Letters , vol. 6, no. 4, pp. 7989–7996, 2021. [21] J. B. Rawlings, D. Q. Mayne, and M. Diehl, Model predictive contr ol: Theory, computation, and design (2nd Edition) . Nob Hill Publishing Madison, WI, 2017. [22] D. Limon, T . Alamo, and E. F . Camacho, “Enlarging the domain of attraction of MPC controllers, ” Automatica , vol. 41, no. 4, pp. 629–635, 2005. [23] H. Xie, L. Dai, Y . Lu, and Y . Xia, “Disturbance rejection MPC framew ork for input-afﬁne nonlinear systems, ” IEEE T ransactions on A utomatic Contr ol , vol. 67, no. 12, pp. 6595–6610, 2021. 20 MPC as a Copilot: A Predictiv e Filter Framework with Safety and Stability Guarantees [24] K. H. Kim, V . T odorovski, and M. Krsti ´ c, “Nonholonomic robot parking by feedback–Part II: Nonmodular , in verse optimal, adapti ve, prescribed/ﬁxed-time and safe designs, ” arXiv preprint , 2025. [25] T . P . Nascimento, C. E. D ´ orea, and L. M. G. Gonc ¸ alves, “Nonholonomic mobile robots’ trajectory tracking model predictiv e control: A survey , ” Robotica , vol. 36, no. 5, pp. 676–696, 2018. 21

MPC as a Copilot: A Predictive Filter Framework with Safety and Stability Guarantees

Original Paper

Comments & Academic Discussion

Leave a Comment

Original Paper

Related Papers

Comments & Academic Discussion

Leave a Comment